zouboan commented on code in PR #8982: URL: https://github.com/apache/nuttx/pull/8982#discussion_r1160803375
########## drivers/sensors/mpu9250.c: ########## @@ -0,0 +1,2045 @@ +/**************************************************************************** + * 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 <nuttx/config.h> + +#include <errno.h> +#include <math.h> +#include <stdio.h> +#include <debug.h> +#include <string.h> +#include <limits.h> +#include <nuttx/mutex.h> +#include <nuttx/signal.h> + +#include <nuttx/compiler.h> +#include <nuttx/kmalloc.h> +#include <nuttx/kthread.h> + +#ifdef CONFIG_MPU9250_SPI +#include <nuttx/spi/spi.h> +#else +#include <nuttx/i2c/i2c_master.h> +#endif +#include <nuttx/fs/fs.h> +#include <nuttx/sensors/mpu9250.h> + +/**************************************************************************** + * 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 accleratormeter full scale */ Review Comment: done ########## drivers/sensors/mpu9250.c: ########## @@ -0,0 +1,2045 @@ +/**************************************************************************** + * 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 <nuttx/config.h> + +#include <errno.h> +#include <math.h> +#include <stdio.h> +#include <debug.h> +#include <string.h> +#include <limits.h> +#include <nuttx/mutex.h> +#include <nuttx/signal.h> + +#include <nuttx/compiler.h> +#include <nuttx/kmalloc.h> +#include <nuttx/kthread.h> + +#ifdef CONFIG_MPU9250_SPI +#include <nuttx/spi/spi.h> +#else +#include <nuttx/i2c/i2c_master.h> +#endif +#include <nuttx/fs/fs.h> +#include <nuttx/sensors/mpu9250.h> + +/**************************************************************************** + * 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 accleratormeter 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 accleratormeter. Review Comment: done -- This is an automated message from the Apache Git Service. To respond to the message, please log on to GitHub and use the URL above to go to the specific comment. To unsubscribe, e-mail: commits-unsubscr...@nuttx.apache.org For queries about this service, please contact Infrastructure at: us...@infra.apache.org
