diff --git a/drivers/sensor/CMakeLists.txt b/drivers/sensor/CMakeLists.txt index 3244ac0d001..fb4dde2e7fa 100644 --- a/drivers/sensor/CMakeLists.txt +++ b/drivers/sensor/CMakeLists.txt @@ -67,6 +67,7 @@ add_subdirectory_ifdef(CONFIG_MCP9808 mcp9808) add_subdirectory_ifdef(CONFIG_MHZ19B mhz19b) add_subdirectory_ifdef(CONFIG_MPR mpr) add_subdirectory_ifdef(CONFIG_MPU6050 mpu6050) +add_subdirectory_ifdef(CONFIG_MPU9250 mpu9250) add_subdirectory_ifdef(CONFIG_MS5607 ms5607) add_subdirectory_ifdef(CONFIG_MS5837 ms5837) add_subdirectory_ifdef(CONFIG_OPT3001 opt3001) diff --git a/drivers/sensor/Kconfig b/drivers/sensor/Kconfig index 6681449292f..2909d735e86 100644 --- a/drivers/sensor/Kconfig +++ b/drivers/sensor/Kconfig @@ -174,6 +174,8 @@ source "drivers/sensor/mpr/Kconfig" source "drivers/sensor/mpu6050/Kconfig" +source "drivers/sensor/mpu9250/Kconfig" + source "drivers/sensor/ms5837/Kconfig" source "drivers/sensor/ms5607/Kconfig" diff --git a/drivers/sensor/mpu9250/CMakeLists.txt b/drivers/sensor/mpu9250/CMakeLists.txt new file mode 100644 index 00000000000..49ba5d8613d --- /dev/null +++ b/drivers/sensor/mpu9250/CMakeLists.txt @@ -0,0 +1,7 @@ +# SPDX-License-Identifier: Apache-2.0 + +zephyr_library() + +zephyr_library_sources(mpu9250.c) +zephyr_library_sources_ifdef(CONFIG_MPU9250_TRIGGER mpu9250_trigger.c) +zephyr_library_sources_ifdef(CONFIG_MPU9250_MAGN_EN ak8963.c) diff --git a/drivers/sensor/mpu9250/Kconfig b/drivers/sensor/mpu9250/Kconfig new file mode 100644 index 00000000000..2f1e9733501 --- /dev/null +++ b/drivers/sensor/mpu9250/Kconfig @@ -0,0 +1,58 @@ +# MPU9250 Nine-Axis Motion Tracking device configuration options + +# Copyright (c) 2021 Nordic Semiconductor ASA +# SPDX-License-Identifier: Apache-2.0 + +menuconfig MPU9250 + bool "MPU9250 Nine-Axis Motion Tracking Device" + depends on I2C + help + Enable driver for MPU9250 I2C-based nine-axis motion tracking device. + +if MPU9250 + +choice + prompt "Trigger mode" + default MPU9250_TRIGGER_GLOBAL_THREAD + help + Specify the type of triggering to be used by the driver. + +config MPU9250_TRIGGER_NONE + bool "No trigger" + +config MPU9250_TRIGGER_GLOBAL_THREAD + bool "Use global thread" + depends on GPIO + select MPU9250_TRIGGER + +config MPU9250_TRIGGER_OWN_THREAD + bool "Use own thread" + depends on GPIO + select MPU9250_TRIGGER + +endchoice + +config MPU9250_TRIGGER + bool + +config MPU9250_THREAD_PRIORITY + int "Thread priority" + depends on MPU9250_TRIGGER_OWN_THREAD + default 10 + help + Priority of thread used by the driver to handle interrupts. + +config MPU9250_THREAD_STACK_SIZE + int "Thread stack size" + depends on MPU9250_TRIGGER_OWN_THREAD + default 1024 + help + Stack size of thread used by the driver to handle interrupts. + +config MPU9250_MAGN_EN + bool "Magnetometer enable" + default y + help + Enable AK8963 builtin magnetometer. + +endif # MPU9250 diff --git a/drivers/sensor/mpu9250/ak8963.c b/drivers/sensor/mpu9250/ak8963.c new file mode 100644 index 00000000000..0321f7a1152 --- /dev/null +++ b/drivers/sensor/mpu9250/ak8963.c @@ -0,0 +1,401 @@ +/* + * Copyright (c) 2021, Nordic Semiconductor ASA + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include +#include +#include + +#include "mpu9250.h" +#include "ak8963.h" + +LOG_MODULE_DECLARE(MPU9250, CONFIG_SENSOR_LOG_LEVEL); + + +#define I2C_READ_FLAG BIT(7) + +#define AK8963_I2C_ADDR 0x0C + +#define AK8963_REG_ID 0x00 +#define AK8963_REG_ID_VAL 0x48 + +#define AK8963_REG_DATA 0x03 + +#define AK8963_ST2_OVRFL_BIT BIT(3) + +#define AK8963_REG_CNTL1 0x0A +#define AK8963_REG_CNTL1_POWERDOWN_VAL 0x00 +#define AK8963_REG_CNTL1_FUSE_ROM_VAL 0x0F +#define AK8963_REG_CNTL1_16BIT_100HZ_VAL 0x16 +#define AK8963_SET_MODE_DELAY_MS 1 + +#define AK8963_REG_CNTL2 0x0B +#define AK8963_REG_CNTL2_RESET_VAL 0x01 +#define AK8963_RESET_DELAY_MS 1 + +#define AK8963_REG_ADJ_DATA_X 0x10 +#define AK8963_REG_ADJ_DATA_Y 0x11 +#define AK8963_REG_ADJ_DATA_Z 0x12 + +#define AK9863_SCALE_TO_UG 1499 + +#define MPU9250_REG_I2C_MST_CTRL 0x24 +#define MPU9250_REG_I2C_MST_CTRL_WAIT_MAG_400KHZ_VAL 0x4D + +#define MPU9250_REG_I2C_SLV0_ADDR 0x25 +#define MPU9250_REG_I2C_SLV0_REG 0x26 +#define MPU9250_REG_I2C_SLV0_CTRL 0x27 +#define MPU9250_REG_I2C_SLV0_DATA0 0x63 +#define MPU9250_REG_READOUT_CTRL_VAL (BIT(7) | 0x07) + +#define MPU9250_REG_USER_CTRL 0x6A +#define MPU9250_REG_USER_CTRL_I2C_MASTERMODE_VAL 0x20 + +#define MPU9250_REG_EXT_DATA00 0x49 + +#define MPU9250_REG_I2C_SLV4_ADDR 0x31 +#define MPU9250_REG_I2C_SLV4_REG 0x32 +#define MPU9250_REG_I2C_SLV4_DO 0x33 +#define MPU9250_REG_I2C_SLV4_CTRL 0x34 +#define MPU9250_REG_I2C_SLV4_CTRL_VAL 0x80 +#define MPU9250_REG_I2C_SLV4_DI 0x35 + +#define MPU9250_I2C_MST_STS 0x36 +#define MPU9250_I2C_MST_STS_SLV4_DONE BIT(6) + + +int ak8963_convert_magn(struct sensor_value *val, int16_t raw_val, + int16_t scale, uint8_t st2) +{ + /* The sensor device returns 10^-9 Teslas after scaling. + * Scale adjusts for calibration data and units + * So sensor instance returns Gauss units + */ + + /* If overflow happens then value is invalid */ + if ((st2 & AK8963_ST2_OVRFL_BIT) != 0) { + LOG_INF("Magnetometer value overflow."); + return -EOVERFLOW; + } + + int32_t scaled_val = (int32_t)raw_val * (int32_t)scale; + + val->val1 = scaled_val / 1000000; + val->val2 = scaled_val % 1000000; + return 0; +} + + +static int ak8963_execute_rw(const struct device *dev, uint8_t reg, bool write) +{ + /* Instruct the MPU9250 to access over its external i2c bus + * given device register with given details + */ + const struct mpu9250_config *cfg = dev->config; + uint8_t mode_bit = 0x00; + uint8_t status; + int ret; + + if (!write) { + mode_bit = I2C_READ_FLAG; + } + + /* Set target i2c address */ + ret = i2c_reg_write_byte_dt(&cfg->i2c, + MPU9250_REG_I2C_SLV4_ADDR, + AK8963_I2C_ADDR | mode_bit); + if (ret < 0) { + LOG_ERR("Failed to write i2c target slave address."); + return ret; + } + + /* Set target i2c register */ + ret = i2c_reg_write_byte_dt(&cfg->i2c, + MPU9250_REG_I2C_SLV4_REG, + reg); + if (ret < 0) { + LOG_ERR("Failed to write i2c target slave register."); + return ret; + } + + /* Initiate transfer */ + ret = i2c_reg_write_byte_dt(&cfg->i2c, + MPU9250_REG_I2C_SLV4_CTRL, + MPU9250_REG_I2C_SLV4_CTRL_VAL); + if (ret < 0) { + LOG_ERR("Failed to initiate i2c slave transfer."); + return ret; + } + + /* Wait for a transfer to be ready */ + do { + ret = i2c_reg_read_byte_dt(&cfg->i2c, + MPU9250_I2C_MST_STS, &status); + if (ret < 0) { + LOG_ERR("Waiting for slave failed."); + return ret; + } + } while (!(status & MPU9250_I2C_MST_STS_SLV4_DONE)); + + return 0; +} + +static int ak8963_read_reg(const struct device *dev, uint8_t reg, uint8_t *data) +{ + const struct mpu9250_config *cfg = dev->config; + int ret; + + /* Execute transfer */ + ret = ak8963_execute_rw(dev, reg, false); + if (ret < 0) { + LOG_ERR("Failed to prepare transfer."); + return ret; + } + + /* Read the result */ + ret = i2c_reg_read_byte_dt(&cfg->i2c, + MPU9250_REG_I2C_SLV4_DI, data); + if (ret < 0) { + LOG_ERR("Failed to read data from slave."); + return ret; + } + + return 0; +} + +static int ak8963_write_reg(const struct device *dev, uint8_t reg, uint8_t data) +{ + const struct mpu9250_config *cfg = dev->config; + int ret; + + /* Set the data to write */ + ret = i2c_reg_write_byte_dt(&cfg->i2c, + MPU9250_REG_I2C_SLV4_DO, data); + if (ret < 0) { + LOG_ERR("Failed to write data to slave."); + return ret; + } + + /* Execute transfer */ + ret = ak8963_execute_rw(dev, reg, true); + if (ret < 0) { + LOG_ERR("Failed to transfer write to slave."); + return ret; + } + + return 0; +} + + +static int ak8963_set_mode(const struct device *dev, uint8_t mode) +{ + int ret; + + ret = ak8963_write_reg(dev, AK8963_REG_CNTL1, mode); + if (ret < 0) { + LOG_ERR("Failed to set AK8963 mode."); + return ret; + } + + /* Wait for mode to change */ + k_msleep(AK8963_SET_MODE_DELAY_MS); + return 0; +} + +static int16_t ak8963_calc_adj(int16_t val) +{ + + /** Datasheet says the actual register value is in 16bit output max + * value of 32760 that corresponds to 4912 uT flux, yielding factor + * of 0.149938. + * + * Now Zephyr unit is Gauss, and conversion is 1T = 10^4G + * -> 0.1499 * 10^4 = 1499 + * So if we multiply with scaling with 1499 the unit is uG. + * + * Calculation from MPU-9250 Register Map and Descriptions + * adj = (((val-128)*0.5)/128)+1 + */ + return ((AK9863_SCALE_TO_UG * (val - 128)) / 256) + AK9863_SCALE_TO_UG; +} + +static int ak8963_fetch_adj(const struct device *dev) +{ + /* Read magnetometer adjustment data from the AK8963 chip */ + struct mpu9250_data *drv_data = dev->data; + uint8_t buf; + int ret; + + /* Change to FUSE access mode to access adjustment registers */ + ret = ak8963_set_mode(dev, AK8963_REG_CNTL1_FUSE_ROM_VAL); + if (ret < 0) { + LOG_ERR("Failed to set chip in fuse access mode."); + return ret; + } + + ret = ak8963_read_reg(dev, AK8963_REG_ADJ_DATA_X, &buf); + if (ret < 0) { + LOG_ERR("Failed to read adjustment data."); + return ret; + } + drv_data->magn_scale_x = ak8963_calc_adj(buf); + + ret = ak8963_read_reg(dev, AK8963_REG_ADJ_DATA_Y, &buf); + if (ret < 0) { + LOG_ERR("Failed to read adjustment data."); + return ret; + } + drv_data->magn_scale_y = ak8963_calc_adj(buf); + + ret = ak8963_read_reg(dev, AK8963_REG_ADJ_DATA_Z, &buf); + if (ret < 0) { + LOG_ERR("Failed to read adjustment data."); + return ret; + } + drv_data->magn_scale_z = ak8963_calc_adj(buf); + + /* Change back to the powerdown mode */ + ret = ak8963_set_mode(dev, AK8963_REG_CNTL1_POWERDOWN_VAL); + if (ret < 0) { + LOG_ERR("Failed to set chip in power down mode."); + return ret; + } + + LOG_DBG("Adjustment values %d %d %d", drv_data->magn_scale_x, + drv_data->magn_scale_y, drv_data->magn_scale_z); + + return 0; +} + +static int ak8963_reset(const struct device *dev) +{ + int ret; + + /* Reset the chip -> reset all settings. */ + ret = ak8963_write_reg(dev, AK8963_REG_CNTL2, + AK8963_REG_CNTL2_RESET_VAL); + if (ret < 0) { + LOG_ERR("Failed to reset AK8963."); + return ret; + } + + /* Wait for reset */ + k_msleep(AK8963_RESET_DELAY_MS); + + return 0; +} + +static int ak8963_init_master(const struct device *dev) +{ + const struct mpu9250_config *cfg = dev->config; + int ret; + + /* Instruct MPU9250 to use its external I2C bus as master */ + ret = i2c_reg_write_byte_dt(&cfg->i2c, + MPU9250_REG_USER_CTRL, + MPU9250_REG_USER_CTRL_I2C_MASTERMODE_VAL); + if (ret < 0) { + LOG_ERR("Failed to set MPU9250 master i2c mode."); + return ret; + } + + /* Set MPU9250 I2C bus as 400kHz and issue interrupt at data ready. */ + ret = i2c_reg_write_byte_dt(&cfg->i2c, + MPU9250_REG_I2C_MST_CTRL, + MPU9250_REG_I2C_MST_CTRL_WAIT_MAG_400KHZ_VAL); + if (ret < 0) { + LOG_ERR("Failed to set MPU9250 master i2c speed."); + return ret; + } + + return 0; +} + +static int ak8963_init_readout(const struct device *dev) +{ + const struct mpu9250_config *cfg = dev->config; + int ret; + + /* Set target i2c address */ + ret = i2c_reg_write_byte_dt(&cfg->i2c, + MPU9250_REG_I2C_SLV0_ADDR, + AK8963_I2C_ADDR | I2C_READ_FLAG); + if (ret < 0) { + LOG_ERR("Failed to set AK8963 slave address."); + return ret; + } + + /* Set target as data registers */ + ret = i2c_reg_write_byte_dt(&cfg->i2c, + MPU9250_REG_I2C_SLV0_REG, AK8963_REG_DATA); + if (ret < 0) { + LOG_ERR("Failed to set AK8963 register address."); + return ret; + } + + /* Initiate readout at sample rate */ + ret = i2c_reg_write_byte_dt(&cfg->i2c, + MPU9250_REG_I2C_SLV0_CTRL, + MPU9250_REG_READOUT_CTRL_VAL); + if (ret < 0) { + LOG_ERR("Failed to init AK8963 value readout."); + return ret; + } + + return 0; +} + +int ak8963_init(const struct device *dev) +{ + uint8_t buf; + int ret; + + ret = ak8963_init_master(dev); + if (ret < 0) { + LOG_ERR("Initializing MPU9250 master mode failed."); + return ret; + } + + ret = ak8963_reset(dev); + if (ret < 0) { + LOG_ERR("Resetting AK8963 failed."); + return ret; + } + + /* First check that the chip says hello */ + ret = ak8963_read_reg(dev, AK8963_REG_ID, &buf); + if (ret < 0) { + LOG_ERR("Failed to read AK8963 chip id."); + return ret; + } + + if (buf != AK8963_REG_ID_VAL) { + LOG_ERR("Invalid AK8963 chip id (0x%X).", buf); + return -ENOTSUP; + } + + /* Fetch calibration data */ + ret = ak8963_fetch_adj(dev); + if (ret < 0) { + LOG_ERR("Calibrating AK8963 failed."); + return ret; + } + + /* Set AK sample rate and resolution */ + ret = ak8963_set_mode(dev, AK8963_REG_CNTL1_16BIT_100HZ_VAL); + if (ret < 0) { + LOG_ERR("Failed set sample rate for AK8963."); + return ret; + } + + /* Init constant readouts at sample rate */ + ret = ak8963_init_readout(dev); + if (ret < 0) { + LOG_ERR("Initializing AK8963 readout failed."); + return ret; + } + + return 0; +} diff --git a/drivers/sensor/mpu9250/ak8963.h b/drivers/sensor/mpu9250/ak8963.h new file mode 100644 index 00000000000..2afe513e50a --- /dev/null +++ b/drivers/sensor/mpu9250/ak8963.h @@ -0,0 +1,21 @@ +/* + * Copyright (c) 2021, Nordic Semiconductor ASA + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#ifndef ZEPHYR_DRIVERS_SENSOR_MPU9250_AK8963_H_ +#define ZEPHYR_DRIVERS_SENSOR_MPU9250_AK8963_H_ + +#include + +#include +#include + + +int ak8963_convert_magn(struct sensor_value *val, int16_t raw_val, + int16_t scale, uint8_t st2); + +int ak8963_init(const struct device *dev); + +#endif /* ZEPHYR_DRIVERS_SENSOR_MPU9250_AK8963_H_ */ diff --git a/drivers/sensor/mpu9250/mpu9250.c b/drivers/sensor/mpu9250/mpu9250.c new file mode 100644 index 00000000000..cecf9612b84 --- /dev/null +++ b/drivers/sensor/mpu9250/mpu9250.c @@ -0,0 +1,366 @@ +/* + * Copyright (c) 2021, Nordic Semiconductor ASA + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#define DT_DRV_COMPAT invensense_mpu9250 + +#include +#include +#include + +#include "mpu9250.h" + +#ifdef CONFIG_MPU9250_MAGN_EN +#include "ak8963.h" +#endif + +LOG_MODULE_REGISTER(MPU9250, CONFIG_SENSOR_LOG_LEVEL); + + +#define MPU9250_REG_CHIP_ID 0x75 +#define MPU9250_CHIP_ID 0x71 + +#define MPU9250_REG_SR_DIV 0x19 + +#define MPU9250_REG_CONFIG 0x1A +#define MPU9250_GYRO_DLPF_MAX 7 + +#define MPU9250_REG_GYRO_CFG 0x1B +#define MPU9250_GYRO_FS_SHIFT 3 +#define MPU9250_GYRO_FS_MAX 3 + +#define MPU9250_REG_ACCEL_CFG 0x1C +#define MPU9250_ACCEL_FS_SHIFT 3 +#define MPU9250_ACCEL_FS_MAX 3 + +#define MPU9250_REG_ACCEL_CFG2 0x1D +#define MPU9250_ACCEL_DLPF_MAX 7 + +#define MPU9250_REG_DATA_START 0x3B + +#define MPU0259_TEMP_SENSITIVITY 334 +#define MPU9250_TEMP_OFFSET 21 + +#define MPU9250_REG_PWR_MGMT1 0x6B +#define MPU9250_SLEEP_EN BIT(6) + + +#ifdef CONFIG_MPU9250_MAGN_EN +#define MPU9250_READ_BUF_SIZE 11 +#else +#define MPU9250_READ_BUF_SIZE 7 +#endif + + +/* see "Accelerometer Measurements" section from register map description */ +static void mpu9250_convert_accel(struct sensor_value *val, int16_t raw_val, + uint16_t sensitivity_shift) +{ + int64_t conv_val; + + conv_val = ((int64_t)raw_val * SENSOR_G) >> sensitivity_shift; + val->val1 = conv_val / 1000000; + val->val2 = conv_val % 1000000; +} + +/* see "Gyroscope Measurements" section from register map description */ +static void mpu9250_convert_gyro(struct sensor_value *val, int16_t raw_val, + uint16_t sensitivity_x10) +{ + int64_t conv_val; + + conv_val = ((int64_t)raw_val * SENSOR_PI * 10) / + (sensitivity_x10 * 180U); + val->val1 = conv_val / 1000000; + val->val2 = conv_val % 1000000; +} + +/* see "Temperature Measurement" section from register map description */ +static inline void mpu9250_convert_temp(struct sensor_value *val, + int16_t raw_val) +{ + /* Temp[*C] = (raw / sensitivity) + offset */ + val->val1 = (raw_val / MPU0259_TEMP_SENSITIVITY) + MPU9250_TEMP_OFFSET; + val->val2 = (((int64_t)(raw_val % MPU0259_TEMP_SENSITIVITY) * 1000000) + / MPU0259_TEMP_SENSITIVITY); + + if (val->val2 < 0) { + val->val1--; + val->val2 += 1000000; + } else if (val->val2 >= 1000000) { + val->val1++; + val->val2 -= 1000000; + } +} + +static int mpu9250_channel_get(const struct device *dev, + enum sensor_channel chan, + struct sensor_value *val) +{ + struct mpu9250_data *drv_data = dev->data; +#ifdef CONFIG_MPU9250_MAGN_EN + int ret; +#endif + + switch (chan) { + case SENSOR_CHAN_ACCEL_XYZ: + mpu9250_convert_accel(val, drv_data->accel_x, + drv_data->accel_sensitivity_shift); + mpu9250_convert_accel(val + 1, drv_data->accel_y, + drv_data->accel_sensitivity_shift); + mpu9250_convert_accel(val + 2, drv_data->accel_z, + drv_data->accel_sensitivity_shift); + break; + case SENSOR_CHAN_ACCEL_X: + mpu9250_convert_accel(val, drv_data->accel_x, + drv_data->accel_sensitivity_shift); + break; + case SENSOR_CHAN_ACCEL_Y: + mpu9250_convert_accel(val, drv_data->accel_y, + drv_data->accel_sensitivity_shift); + break; + case SENSOR_CHAN_ACCEL_Z: + mpu9250_convert_accel(val, drv_data->accel_z, + drv_data->accel_sensitivity_shift); + break; + case SENSOR_CHAN_GYRO_XYZ: + mpu9250_convert_gyro(val, drv_data->gyro_x, + drv_data->gyro_sensitivity_x10); + mpu9250_convert_gyro(val + 1, drv_data->gyro_y, + drv_data->gyro_sensitivity_x10); + mpu9250_convert_gyro(val + 2, drv_data->gyro_z, + drv_data->gyro_sensitivity_x10); + break; + case SENSOR_CHAN_GYRO_X: + mpu9250_convert_gyro(val, drv_data->gyro_x, + drv_data->gyro_sensitivity_x10); + break; + case SENSOR_CHAN_GYRO_Y: + mpu9250_convert_gyro(val, drv_data->gyro_y, + drv_data->gyro_sensitivity_x10); + break; + case SENSOR_CHAN_GYRO_Z: + mpu9250_convert_gyro(val, drv_data->gyro_z, + drv_data->gyro_sensitivity_x10); + break; +#ifdef CONFIG_MPU9250_MAGN_EN + case SENSOR_CHAN_MAGN_XYZ: + ret = ak8963_convert_magn(val, drv_data->magn_x, + drv_data->magn_scale_x, + drv_data->magn_st2); + if (ret < 0) { + return ret; + } + ret = ak8963_convert_magn(val + 1, drv_data->magn_y, + drv_data->magn_scale_y, + drv_data->magn_st2); + if (ret < 0) { + return ret; + } + ret = ak8963_convert_magn(val + 2, drv_data->magn_z, + drv_data->magn_scale_z, + drv_data->magn_st2); + return ret; + case SENSOR_CHAN_MAGN_X: + return ak8963_convert_magn(val, drv_data->magn_x, + drv_data->magn_scale_x, + drv_data->magn_st2); + case SENSOR_CHAN_MAGN_Y: + return ak8963_convert_magn(val, drv_data->magn_y, + drv_data->magn_scale_y, + drv_data->magn_st2); + case SENSOR_CHAN_MAGN_Z: + return ak8963_convert_magn(val, drv_data->magn_z, + drv_data->magn_scale_z, + drv_data->magn_st2); + case SENSOR_CHAN_DIE_TEMP: + mpu9250_convert_temp(val, drv_data->temp); + break; +#endif + default: + return -ENOTSUP; + } + + return 0; +} + +static int mpu9250_sample_fetch(const struct device *dev, + enum sensor_channel chan) +{ + struct mpu9250_data *drv_data = dev->data; + const struct mpu9250_config *cfg = dev->config; + int16_t buf[MPU9250_READ_BUF_SIZE]; + int ret; + + ret = i2c_burst_read_dt(&cfg->i2c, + MPU9250_REG_DATA_START, (uint8_t *)buf, + sizeof(buf)); + if (ret < 0) { + LOG_ERR("Failed to read data sample."); + return ret; + } + + drv_data->accel_x = sys_be16_to_cpu(buf[0]); + drv_data->accel_y = sys_be16_to_cpu(buf[1]); + drv_data->accel_z = sys_be16_to_cpu(buf[2]); + drv_data->temp = sys_be16_to_cpu(buf[3]); + drv_data->gyro_x = sys_be16_to_cpu(buf[4]); + drv_data->gyro_y = sys_be16_to_cpu(buf[5]); + drv_data->gyro_z = sys_be16_to_cpu(buf[6]); +#ifdef CONFIG_MPU9250_MAGN_EN + drv_data->magn_x = sys_be16_to_cpu(buf[7]); + drv_data->magn_y = sys_be16_to_cpu(buf[8]); + drv_data->magn_z = sys_be16_to_cpu(buf[9]); + drv_data->magn_st2 = ((uint8_t *)buf)[20]; + LOG_DBG("magn_st2: %u", drv_data->magn_st2); +#endif + + return 0; +} + +static const struct sensor_driver_api mpu9250_driver_api = { +#if CONFIG_MPU9250_TRIGGER + .trigger_set = mpu9250_trigger_set, +#endif + .sample_fetch = mpu9250_sample_fetch, + .channel_get = mpu9250_channel_get, +}; + +/* measured in degrees/sec x10 to avoid floating point */ +static const uint16_t mpu9250_gyro_sensitivity_x10[] = { + 1310, 655, 328, 164 +}; + +static int mpu9250_init(const struct device *dev) +{ + struct mpu9250_data *drv_data = dev->data; + const struct mpu9250_config *cfg = dev->config; + uint8_t id; + int ret; + + if (!device_is_ready(cfg->i2c.bus)) { + LOG_ERR("I2C dev %s not ready", cfg->i2c.bus->name); + return -ENODEV; + } + + /* check chip ID */ + ret = i2c_reg_read_byte_dt(&cfg->i2c, MPU9250_REG_CHIP_ID, &id); + if (ret < 0) { + LOG_ERR("Failed to read chip ID."); + return ret; + } + + if (id != MPU9250_CHIP_ID) { + LOG_ERR("Invalid chip ID."); + return -ENOTSUP; + } + + /* wake up chip */ + ret = i2c_reg_update_byte_dt(&cfg->i2c, + MPU9250_REG_PWR_MGMT1, + MPU9250_SLEEP_EN, 0); + if (ret < 0) { + LOG_ERR("Failed to wake up chip."); + return ret; + } + + if (cfg->accel_fs > MPU9250_ACCEL_FS_MAX) { + LOG_ERR("Accel FS is too big: %d", cfg->accel_fs); + return -EINVAL; + } + + ret = i2c_reg_write_byte_dt(&cfg->i2c, MPU9250_REG_ACCEL_CFG, + cfg->accel_fs << MPU9250_ACCEL_FS_SHIFT); + if (ret < 0) { + LOG_ERR("Failed to write accel full-scale range."); + return ret; + } + drv_data->accel_sensitivity_shift = 14 - cfg->accel_fs; + + if (cfg->gyro_fs > MPU9250_GYRO_FS_MAX) { + LOG_ERR("Gyro FS is too big: %d", cfg->accel_fs); + return -EINVAL; + } + + ret = i2c_reg_write_byte_dt(&cfg->i2c, MPU9250_REG_GYRO_CFG, + cfg->gyro_fs << MPU9250_GYRO_FS_SHIFT); + if (ret < 0) { + LOG_ERR("Failed to write gyro full-scale range."); + return ret; + } + + if (cfg->gyro_dlpf > MPU9250_GYRO_DLPF_MAX) { + LOG_ERR("Gyro DLPF is too big: %d", cfg->gyro_dlpf); + return -EINVAL; + } + + ret = i2c_reg_write_byte_dt(&cfg->i2c, MPU9250_REG_CONFIG, + cfg->gyro_dlpf); + if (ret < 0) { + LOG_ERR("Failed to write gyro digital LPF settings."); + return ret; + } + + if (cfg->accel_dlpf > MPU9250_ACCEL_DLPF_MAX) { + LOG_ERR("Accel DLPF is too big: %d", cfg->accel_dlpf); + return -EINVAL; + } + + ret = i2c_reg_write_byte_dt(&cfg->i2c, MPU9250_REG_ACCEL_CFG2, + cfg->gyro_dlpf); + if (ret < 0) { + LOG_ERR("Failed to write accel digital LPF settings."); + return ret; + } + + ret = i2c_reg_write_byte_dt(&cfg->i2c, MPU9250_REG_SR_DIV, + cfg->gyro_sr_div); + if (ret < 0) { + LOG_ERR("Failed to write gyro ODR divider."); + return ret; + } + + drv_data->gyro_sensitivity_x10 = + mpu9250_gyro_sensitivity_x10[cfg->gyro_fs]; + +#ifdef CONFIG_MPU9250_MAGN_EN + ret = ak8963_init(dev); + if (ret < 0) { + LOG_ERR("Failed to initialize AK8963."); + return ret; + } +#endif + +#ifdef CONFIG_MPU9250_TRIGGER + ret = mpu9250_init_interrupt(dev); + if (ret < 0) { + LOG_ERR("Failed to initialize interrupts."); + return ret; + } +#endif + + return 0; +} + + +#define INIT_MPU9250_INST(inst) \ + static struct mpu9250_data mpu9250_data_##inst; \ + static const struct mpu9250_config mpu9250_cfg_##inst = { \ + .i2c = I2C_DT_SPEC_INST_GET(inst), \ + .gyro_sr_div = DT_INST_PROP(inst, gyro_sr_div), \ + .gyro_dlpf = DT_INST_ENUM_IDX(inst, gyro_dlpf), \ + .gyro_fs = DT_INST_ENUM_IDX(inst, gyro_fs), \ + .accel_fs = DT_INST_ENUM_IDX(inst, accel_fs), \ + .accel_dlpf = DT_INST_ENUM_IDX(inst, accel_dlpf), \ + IF_ENABLED(CONFIG_MPU9250_TRIGGER, \ + (.int_pin = GPIO_DT_SPEC_INST_GET(inst, irq_gpios))) \ + }; \ + \ + DEVICE_DT_INST_DEFINE(inst, mpu9250_init, NULL, \ + &mpu9250_data_##inst, &mpu9250_cfg_##inst,\ + POST_KERNEL, CONFIG_SENSOR_INIT_PRIORITY, \ + &mpu9250_driver_api); + +DT_INST_FOREACH_STATUS_OKAY(INIT_MPU9250_INST) diff --git a/drivers/sensor/mpu9250/mpu9250.h b/drivers/sensor/mpu9250/mpu9250.h new file mode 100644 index 00000000000..b636ded7ecc --- /dev/null +++ b/drivers/sensor/mpu9250/mpu9250.h @@ -0,0 +1,78 @@ +/* + * Copyright (c) 2021, Nordic Semiconductor ASA + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#ifndef ZEPHYR_DRIVERS_SENSOR_MPU9250_MPU9250_H_ +#define ZEPHYR_DRIVERS_SENSOR_MPU9250_MPU9250_H_ + +#include +#include +#include +#include +#include +#include + +struct mpu9250_data { + int16_t accel_x; + int16_t accel_y; + int16_t accel_z; + uint16_t accel_sensitivity_shift; + + int16_t temp; + + int16_t gyro_x; + int16_t gyro_y; + int16_t gyro_z; + uint16_t gyro_sensitivity_x10; + +#ifdef CONFIG_MPU9250_MAGN_EN + int16_t magn_x; + int16_t magn_scale_x; + int16_t magn_y; + int16_t magn_scale_y; + int16_t magn_z; + int16_t magn_scale_z; + uint8_t magn_st2; +#endif + +#ifdef CONFIG_MPU9250_TRIGGER + const struct device *dev; + struct gpio_callback gpio_cb; + + struct sensor_trigger data_ready_trigger; + sensor_trigger_handler_t data_ready_handler; + +#if defined(CONFIG_MPU9250_TRIGGER_OWN_THREAD) + K_KERNEL_STACK_MEMBER(thread_stack, CONFIG_MPU9250_THREAD_STACK_SIZE); + struct k_thread thread; + struct k_sem gpio_sem; +#elif defined(CONFIG_MPU9250_TRIGGER_GLOBAL_THREAD) + struct k_work work; +#endif + +#endif /* CONFIG_MPU9250_TRIGGER */ +}; + +struct mpu9250_config { + const struct i2c_dt_spec i2c; + uint8_t gyro_sr_div; + uint8_t gyro_dlpf; + uint8_t gyro_fs; + uint8_t accel_fs; + uint8_t accel_dlpf; +#ifdef CONFIG_MPU9250_TRIGGER + const struct gpio_dt_spec int_pin; +#endif /* CONFIG_MPU9250_TRIGGER */ +}; + +#ifdef CONFIG_MPU9250_TRIGGER +int mpu9250_trigger_set(const struct device *dev, + const struct sensor_trigger *trig, + sensor_trigger_handler_t handler); + +int mpu9250_init_interrupt(const struct device *dev); +#endif + +#endif /* ZEPHYR_DRIVERS_SENSOR_MPU9250_MPU9250_H_ */ diff --git a/drivers/sensor/mpu9250/mpu9250_trigger.c b/drivers/sensor/mpu9250/mpu9250_trigger.c new file mode 100644 index 00000000000..3bdeda4f747 --- /dev/null +++ b/drivers/sensor/mpu9250/mpu9250_trigger.c @@ -0,0 +1,178 @@ +/* + * Copyright (c) 2021, Nordic Semiconductor ASA + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include +#include + +#include "mpu9250.h" + +LOG_MODULE_DECLARE(MPU9250, CONFIG_SENSOR_LOG_LEVEL); + + +#define MPU9250_REG_INT_EN 0x38 +#define MPU9250_DRDY_EN BIT(0) + + +int mpu9250_trigger_set(const struct device *dev, + const struct sensor_trigger *trig, + sensor_trigger_handler_t handler) +{ + struct mpu9250_data *drv_data = dev->data; + const struct mpu9250_config *cfg = dev->config; + int ret; + + if (trig->type != SENSOR_TRIG_DATA_READY) { + return -ENOTSUP; + } + + ret = gpio_pin_interrupt_configure_dt(&cfg->int_pin, GPIO_INT_DISABLE); + if (ret < 0) { + LOG_ERR("Failed to disable gpio interrupt."); + return ret; + } + + drv_data->data_ready_handler = handler; + if (handler == NULL) { + return 0; + } + + drv_data->data_ready_trigger = *trig; + + ret = gpio_pin_interrupt_configure_dt(&cfg->int_pin, + GPIO_INT_EDGE_TO_ACTIVE); + if (ret < 0) { + LOG_ERR("Failed to enable gpio interrupt."); + return ret; + } + + return 0; +} + +static void mpu9250_gpio_callback(const struct device *dev, + struct gpio_callback *cb, uint32_t pins) +{ + struct mpu9250_data *drv_data = + CONTAINER_OF(cb, struct mpu9250_data, gpio_cb); + const struct mpu9250_config *cfg = drv_data->dev->config; + int ret; + + ARG_UNUSED(pins); + + ret = gpio_pin_interrupt_configure_dt(&cfg->int_pin, GPIO_INT_DISABLE); + if (ret < 0) { + LOG_ERR("Disabling gpio interrupt failed with err: %d", ret); + return; + } + +#if defined(CONFIG_MPU9250_TRIGGER_OWN_THREAD) + k_sem_give(&drv_data->gpio_sem); +#elif defined(CONFIG_MPU9250_TRIGGER_GLOBAL_THREAD) + k_work_submit(&drv_data->work); +#endif +} + +static void mpu9250_thread_cb(const struct device *dev) +{ + struct mpu9250_data *drv_data = dev->data; + const struct mpu9250_config *cfg = dev->config; + int ret; + + if (drv_data->data_ready_handler != NULL) { + drv_data->data_ready_handler(dev, + &drv_data->data_ready_trigger); + } + + ret = gpio_pin_interrupt_configure_dt(&cfg->int_pin, + GPIO_INT_EDGE_TO_ACTIVE); + if (ret < 0) { + LOG_ERR("Enabling gpio interrupt failed with err: %d", ret); + } + +} + +#ifdef CONFIG_MPU9250_TRIGGER_OWN_THREAD +static void mpu9250_thread(struct mpu9250_data *drv_data) +{ + while (1) { + k_sem_take(&drv_data->gpio_sem, K_FOREVER); + mpu9250_thread_cb(drv_data->dev); + } +} +#endif + +#ifdef CONFIG_MPU9250_TRIGGER_GLOBAL_THREAD +static void mpu9250_work_cb(struct k_work *work) +{ + struct mpu9250_data *drv_data = + CONTAINER_OF(work, struct mpu9250_data, work); + + mpu9250_thread_cb(drv_data->dev); +} +#endif + +int mpu9250_init_interrupt(const struct device *dev) +{ + struct mpu9250_data *drv_data = dev->data; + const struct mpu9250_config *cfg = dev->config; + int ret; + + /* setup data ready gpio interrupt */ + if (!device_is_ready(cfg->int_pin.port)) { + LOG_ERR("Interrupt pin is not ready."); + return -EIO; + } + + drv_data->dev = dev; + + ret = gpio_pin_configure_dt(&cfg->int_pin, GPIO_INPUT); + if (ret < 0) { + LOG_ERR("Failed to configure interrupt pin."); + return ret; + } + + gpio_init_callback(&drv_data->gpio_cb, + mpu9250_gpio_callback, + BIT(cfg->int_pin.pin)); + + ret = gpio_add_callback(cfg->int_pin.port, &drv_data->gpio_cb); + if (ret < 0) { + LOG_ERR("Failed to set gpio callback."); + return ret; + } + + /* enable data ready interrupt */ + ret = i2c_reg_write_byte_dt(&cfg->i2c, MPU9250_REG_INT_EN, + MPU9250_DRDY_EN); + if (ret < 0) { + LOG_ERR("Failed to enable data ready interrupt."); + return ret; + } + +#if defined(CONFIG_MPU9250_TRIGGER_OWN_THREAD) + ret = k_sem_init(&drv_data->gpio_sem, 0, K_SEM_MAX_LIMIT); + if (ret < 0) { + LOG_ERR("Failed to enable semaphore"); + return ret; + } + + k_thread_create(&drv_data->thread, drv_data->thread_stack, + CONFIG_MPU9250_THREAD_STACK_SIZE, + (k_thread_entry_t)mpu9250_thread, drv_data, + NULL, NULL, K_PRIO_COOP(CONFIG_MPU9250_THREAD_PRIORITY), + 0, K_NO_WAIT); +#elif defined(CONFIG_MPU9250_TRIGGER_GLOBAL_THREAD) + drv_data->work.handler = mpu9250_work_cb; +#endif + + ret = gpio_pin_interrupt_configure_dt(&cfg->int_pin, + GPIO_INT_EDGE_TO_ACTIVE); + if (ret < 0) { + LOG_ERR("Failed to enable interrupt"); + return ret; + } + + return 0; +} diff --git a/dts/bindings/sensor/invensense,mpu9250.yaml b/dts/bindings/sensor/invensense,mpu9250.yaml new file mode 100644 index 00000000000..fb9590554f3 --- /dev/null +++ b/dts/bindings/sensor/invensense,mpu9250.yaml @@ -0,0 +1,85 @@ +# Copyright (c) 2021 Nordic Semiconductor +# SPDX-License-Identifier: Apache-2.0 + +description: | + InvenSense MPU-9250 Nine-Axis (Gyro + Accelerometer + Compass). See more + info at https://www.invensense.com/products/motion-tracking/9-axis/mpu-9250/ + +compatible: "invensense,mpu9250" + +include: i2c-device.yaml + +properties: + irq-gpios: + type: phandle-array + required: false + description: | + The INT signal default configuration is active-high. The + property value should ensure the flags properly describe the + signal that is presented to the driver. + This property is required when the trigger mode is used. + + gyro-sr-div: + type: int + required: true + description: | + Default gyrscope sample rate divider. This divider is only effective + when gyro-dlpf is in range 5-184. + rate = sample_rate / (1 + gyro-sr-div) + Valid range: 0 - 255 + + gyro-dlpf: + type: int + required: true + description: | + Default digital low pass filter frequency of gyroscope. + Maps to DLPF_CFG field in Configuration setting. + enum: + - 250 + - 184 + - 92 + - 41 + - 20 + - 10 + - 5 + - 3600 + + gyro-fs: + type: int + required: true + description: | + Default full scale of gyroscope. (Unit - DPS). + Maps to GYRO_FS_SEL field in Gyroscope Configuration setting. + enum: + - 250 + - 500 + - 1000 + - 2000 + + accel-fs: + type: int + required: true + description: | + Default full scale of accelerometer. (Unit - g) + Maps to ACCEL_FS_SEL field in Accelerometer Configuration setting + enum: + - 2 + - 4 + - 8 + - 16 + + accel-dlpf: + type: string + required: true + description: | + Default digital low pass filter frequency of accelerometer. + Maps to DLPF_CFG field in Accelerometer Configuration 2 setting. + enum: + - "218.1" + - "218.1a" + - "99" + - "44.8" + - "21.2" + - "10.2" + - "5.05" + - "420" diff --git a/tests/drivers/build_all/sensor/i2c.dtsi b/tests/drivers/build_all/sensor/i2c.dtsi index 052ec758364..1b2fe44f0a8 100644 --- a/tests/drivers/build_all/sensor/i2c.dtsi +++ b/tests/drivers/build_all/sensor/i2c.dtsi @@ -154,6 +154,18 @@ test_i2c_mpu9150: mpu9150@16 { reg = <0x16>; }; +test_i2c_mpu9250: mpu9250@1e { + compatible = "invensense,mpu9250"; + reg = <0x1e>; + irq-gpios = <&test_gpio 0 0>; + gyro-sr-div = <10>; + gyro-dlpf = <5>; + gyro-fs = <250>; + accel-fs = <2>; + accel-dlpf="5.05"; + label = "MPU9250"; +}; + test_i2c_ina219: ina219@40 { compatible = "ti,ina219"; label = "INA219"; diff --git a/tests/drivers/build_all/sensor/prj.conf b/tests/drivers/build_all/sensor/prj.conf index ef269aaf698..f2ac144eb7e 100644 --- a/tests/drivers/build_all/sensor/prj.conf +++ b/tests/drivers/build_all/sensor/prj.conf @@ -72,6 +72,7 @@ CONFIG_MAX6675=y CONFIG_MCP9808=y CONFIG_MPR=y CONFIG_MPU6050=y +CONFIG_MPU9250=y CONFIG_MS5607=y CONFIG_MS5837=y CONFIG_OPT3001=y diff --git a/tests/drivers/build_all/sensor/sensors_trigger_own.conf b/tests/drivers/build_all/sensor/sensors_trigger_own.conf index 28a05c164b8..7c77279dcda 100644 --- a/tests/drivers/build_all/sensor/sensors_trigger_own.conf +++ b/tests/drivers/build_all/sensor/sensors_trigger_own.conf @@ -30,6 +30,7 @@ CONFIG_LSM9DS0_GYRO_TRIGGER_DRDY=y CONFIG_LSM9DS0_GYRO_TRIGGERS=y CONFIG_MCP9808_TRIGGER_OWN_THREAD=y CONFIG_MPU6050_TRIGGER_OWN_THREAD=y +CONFIG_MPU9250_TRIGGER_OWN_THREAD=y CONFIG_SHT3XD_TRIGGER_OWN_THREAD=y CONFIG_SM351LT_TRIGGER_OWN_THREAD=y CONFIG_STTS751_TRIGGER_OWN_THREAD=y