From 2ae6b7bd75414ba2a73f7e27f31fdebe35188efd Mon Sep 17 00:00:00 2001 From: Tom Burdick Date: Mon, 6 Feb 2023 15:33:09 -0600 Subject: [PATCH] sensor: icm42688 configuration and regmap fixes The int config and fifo config register addresses were wrong, fix those. Adds lots of debug information (when LOG_DBG=y) to the configuration of the device which is incredibly helpful for diagnosing configuration issues. Disables the device interrupts while reconfiguring. Adds a safely reconfigure function which will rollback to previous configuration on misconfiguration. Signed-off-by: Tom Burdick --- drivers/sensor/icm42688/icm42688.h | 16 +- drivers/sensor/icm42688/icm42688_common.c | 184 +++++++++++++++------- drivers/sensor/icm42688/icm42688_reg.h | 20 ++- 3 files changed, 162 insertions(+), 58 deletions(-) diff --git a/drivers/sensor/icm42688/icm42688.h b/drivers/sensor/icm42688/icm42688.h index 17c607ef7e..3c17f20724 100644 --- a/drivers/sensor/icm42688/icm42688.h +++ b/drivers/sensor/icm42688/icm42688.h @@ -10,7 +10,6 @@ #include #include #include -#include #include #include @@ -425,6 +424,20 @@ int icm42688_reset(const struct device *dev); */ int icm42688_configure(const struct device *dev, struct icm42688_cfg *cfg); + +/** + * @brief Safely (re)Configure the sensor with the given configuration + * + * Will rollback to prior configuration if new configuration is invalid + * + * @param dev icm42688 device pointer + * @param cfg icm42688_cfg pointer + * + * @retval 0 success + * @retval -errno Error + */ +int icm42688_safely_configure(const struct device *dev, struct icm42688_cfg *cfg); + /** * @brief Reads all channels * @@ -636,4 +649,5 @@ static inline void icm42688_temp_c(int32_t in, int32_t *out_c, uint32_t *out_uc) /* Shift whole celsius 25 degress */ *out_c = *out_c + 25; } + #endif /* ZEPHYR_DRIVERS_SENSOR_ICM42688_H_ */ diff --git a/drivers/sensor/icm42688/icm42688_common.c b/drivers/sensor/icm42688/icm42688_common.c index 3768b83df6..26625aa2e2 100644 --- a/drivers/sensor/icm42688/icm42688_common.c +++ b/drivers/sensor/icm42688/icm42688_common.c @@ -69,6 +69,9 @@ int icm42688_configure(const struct device *dev, struct icm42688_cfg *cfg) const struct icm42688_dev_cfg *dev_cfg = dev->config; int res; + /* Disable interrupts, reconfigured at end */ + res = icm42688_spi_single_write(&dev_cfg->spi, REG_INT_SOURCE0, 0); + /* if fifo is enabled right now, disable and flush */ if (dev_data->cfg.fifo_en) { res = icm42688_spi_single_write(&dev_cfg->spi, REG_FIFO_CONFIG, @@ -79,9 +82,6 @@ int icm42688_configure(const struct device *dev, struct icm42688_cfg *cfg) return -EINVAL; } - /* TODO ideally this might read out the FIFO one last time and call - * out to the application with the data there prior to reconfiguring? - */ res = icm42688_spi_single_write(&dev_cfg->spi, REG_SIGNAL_PATH_RESET, FIELD_PREP(BIT_FIFO_FLUSH, 1)); @@ -94,45 +94,42 @@ int icm42688_configure(const struct device *dev, struct icm42688_cfg *cfg) /* TODO maybe do the next few steps intelligently by checking current config */ /* Power management to set gyro/accel modes */ - res = icm42688_spi_single_write(&dev_cfg->spi, REG_PWR_MGMT0, - FIELD_PREP(MASK_GYRO_MODE, cfg->gyro_mode) | - FIELD_PREP(MASK_ACCEL_MODE, cfg->accel_mode) | - FIELD_PREP(BIT_TEMP_DIS, cfg->temp_dis)); + uint8_t pwr_mgmt0 = FIELD_PREP(MASK_GYRO_MODE, cfg->gyro_mode) | + FIELD_PREP(MASK_ACCEL_MODE, cfg->accel_mode) | + FIELD_PREP(BIT_TEMP_DIS, cfg->temp_dis); + + LOG_DBG("PWR_MGMT0 (0x%x) 0x%x", REG_PWR_MGMT0, pwr_mgmt0); + res = icm42688_spi_single_write(&dev_cfg->spi, REG_PWR_MGMT0, pwr_mgmt0); if (res != 0) { LOG_ERR("Error writing PWR_MGMT0"); return -EINVAL; } - dev_data->cfg.gyro_mode = cfg->gyro_mode; - dev_data->cfg.accel_mode = cfg->accel_mode; - dev_data->cfg.temp_dis = cfg->temp_dis; /* Need to wait at least 200us before updating more registers * see datasheet 14.36 */ k_busy_wait(250); - res = icm42688_spi_single_write(&dev_cfg->spi, REG_ACCEL_CONFIG0, - FIELD_PREP(MASK_ACCEL_ODR, cfg->accel_odr) | - FIELD_PREP(MASK_ACCEL_UI_FS_SEL, cfg->accel_fs)); + uint8_t accel_config0 = FIELD_PREP(MASK_ACCEL_ODR, cfg->accel_odr) | + FIELD_PREP(MASK_ACCEL_UI_FS_SEL, cfg->accel_fs); + LOG_DBG("ACCEL_CONFIG0 (0x%x) 0x%x", REG_ACCEL_CONFIG0, accel_config0); + res = icm42688_spi_single_write(&dev_cfg->spi, REG_ACCEL_CONFIG0, accel_config0); if (res != 0) { LOG_ERR("Error writing ACCEL_CONFIG0"); return -EINVAL; } - dev_data->cfg.accel_odr = cfg->accel_odr; - dev_data->cfg.accel_fs = cfg->accel_fs; - res = icm42688_spi_single_write(&dev_cfg->spi, REG_GYRO_CONFIG0, - FIELD_PREP(MASK_GYRO_ODR, cfg->gyro_odr) | - FIELD_PREP(MASK_GYRO_UI_FS_SEL, cfg->gyro_fs)); + uint8_t gyro_config0 = FIELD_PREP(MASK_GYRO_ODR, cfg->gyro_odr) | + FIELD_PREP(MASK_GYRO_UI_FS_SEL, cfg->gyro_fs); + LOG_DBG("GYRO_CONFIG0 (0x%x) 0x%x", REG_GYRO_CONFIG0, gyro_config0); + res = icm42688_spi_single_write(&dev_cfg->spi, REG_GYRO_CONFIG0, gyro_config0); if (res != 0) { LOG_ERR("Error writing GYRO_CONFIG0"); return -EINVAL; } - dev_data->cfg.gyro_odr = cfg->gyro_odr; - dev_data->cfg.gyro_fs = cfg->gyro_fs; /* * Accelerometer sensor need at least 10ms startup time @@ -140,61 +137,138 @@ int icm42688_configure(const struct device *dev, struct icm42688_cfg *cfg) */ k_msleep(50); + /* Ensure FIFO is in bypass mode */ + uint8_t fifo_config_bypass = FIELD_PREP(MASK_FIFO_MODE, BIT_FIFO_MODE_BYPASS); + + LOG_DBG("FIFO_CONFIG (0x%x) 0x%x", REG_FIFO_CONFIG, fifo_config_bypass); + res = icm42688_spi_single_write(&dev_cfg->spi, REG_FIFO_CONFIG, fifo_config_bypass); + if (res != 0) { + LOG_ERR("Error writing FIFO_CONFIG"); + return -EINVAL; + } + + /* Disable FSYNC */ + uint8_t tmst_config; + + res = icm42688_spi_single_write(&dev_cfg->spi, REG_FSYNC_CONFIG, 0); + if (res != 0) { + LOG_ERR("Error writing FSYNC_CONFIG"); + return -EINVAL; + } + res = icm42688_spi_read(&dev_cfg->spi, REG_TMST_CONFIG, &tmst_config, 1); + if (res != 0) { + LOG_ERR("Error reading TMST_CONFIG"); + return -EINVAL; + } + res = icm42688_spi_single_write(&dev_cfg->spi, REG_TMST_CONFIG, tmst_config & ~BIT(1)); + if (res != 0) { + LOG_ERR("Error writing TMST_CONFIG"); + return -EINVAL; + } + + /* Pulse mode with async reset (resets interrupt line on int status read) */ + res = icm42688_spi_single_write(&dev_cfg->spi, REG_INT_CONFIG, + BIT_INT1_DRIVE_CIRCUIT | BIT_INT1_POLARITY); + if (res) { + LOG_ERR("Error writing to INT_CONFIG"); + return res; + } + + uint8_t int_config1 = 0; + + if (cfg->fifo_en && (cfg->accel_odr <= ICM42688_ACCEL_ODR_4000 || + cfg->gyro_odr <= ICM42688_GYRO_ODR_4000)) { + int_config1 = FIELD_PREP(BIT_INT_TPULSE_DURATION, 1) | + FIELD_PREP(BIT_INT_TDEASSERT_DISABLE, 1); + } + + res = icm42688_spi_single_write(&dev_cfg->spi, REG_INT_CONFIG1, int_config1); + if (res) { + LOG_ERR("Error writing to INT_CONFIG1"); + return res; + } + /* fifo configuration steps if desired */ if (cfg->fifo_en) { - /* Set watermark and interrupt handling first */ - res = icm42688_spi_single_write(&dev_cfg->spi, REG_FIFO_CONFIG2, - cfg->fifo_wm & 0xFF); + LOG_INF("FIFO ENABLED"); + /* Setup desired FIFO packet fields, maybe should base this on the other + * temp/accel/gyro en fields in cfg + */ + uint8_t fifo_cfg1 = + FIELD_PREP(BIT_FIFO_TEMP_EN, 1) | FIELD_PREP(BIT_FIFO_GYRO_EN, 1) | + FIELD_PREP(BIT_FIFO_ACCEL_EN, 1) | FIELD_PREP(BIT_FIFO_TMST_FSYNC_EN, 1); + + LOG_DBG("FIFO_CONFIG1 (0x%x) 0x%x", REG_FIFO_CONFIG1, fifo_cfg1); + res = icm42688_spi_single_write(&dev_cfg->spi, REG_FIFO_CONFIG1, fifo_cfg1); + if (res != 0) { + LOG_ERR("Error writing FIFO_CONFIG1"); + return -EINVAL; + } + + /* Set watermark and interrupt handling first */ + uint8_t fifo_wml = (cfg->fifo_wm) & 0xFF; + + LOG_DBG("FIFO_CONFIG2( (0x%x)) (WM Low) 0x%x", REG_FIFO_CONFIG2, fifo_wml); + res = icm42688_spi_single_write(&dev_cfg->spi, REG_FIFO_CONFIG2, fifo_wml); if (res != 0) { LOG_ERR("Error writing FIFO_CONFIG2"); return -EINVAL; } - res = icm42688_spi_single_write(&dev_cfg->spi, REG_FIFO_CONFIG3, - (cfg->fifo_wm >> 8) & 0x0F); + uint8_t fifo_wmh = (cfg->fifo_wm >> 8) & 0x0F; + LOG_DBG("FIFO_CONFIG3 (0x%x) (WM High) 0x%x", REG_FIFO_CONFIG3, fifo_wmh); + res = icm42688_spi_single_write(&dev_cfg->spi, REG_FIFO_CONFIG3, fifo_wmh); if (res != 0) { LOG_ERR("Error writing FIFO_CONFIG3"); return -EINVAL; } - dev_data->cfg.fifo_wm = cfg->fifo_wm; - - /* TODO we have two interrupt lines, either can be used for watermark, choose 1 for - * now... - */ - res = icm42688_spi_single_write(&dev_cfg->spi, REG_INT_SOURCE0, - FIELD_PREP(BIT_FIFO_THS_INT1_EN, 1)); - - if (res != 0) { - LOG_ERR("Error writing INT0_SOURCE"); - return -EINVAL; - } - - /* Setup desired FIFO packet fields, maybe should base this on the other - * temp/accel/gyro en fields in cfg - */ - res = icm42688_spi_single_write( - &dev_cfg->spi, REG_FIFO_CONFIG1, - FIELD_PREP(BIT_FIFO_HIRES_EN, cfg->fifo_hires ? 1 : 0) | - FIELD_PREP(BIT_FIFO_TEMP_EN, 1) | FIELD_PREP(BIT_FIFO_GYRO_EN, 1) | - FIELD_PREP(BIT_FIFO_ACCEL_EN, 1)); - - if (res != 0) { - LOG_ERR("Error writing FIFO_CONFIG1"); - return -EINVAL; - } - dev_data->cfg.fifo_hires = cfg->fifo_hires; /* Begin streaming */ - res = icm42688_spi_single_write(&dev_cfg->spi, REG_FIFO_CONFIG, - FIELD_PREP(MASK_FIFO_MODE, BIT_FIFO_MODE_STREAM)); - dev_data->cfg.fifo_en = true; + uint8_t fifo_config = FIELD_PREP(MASK_FIFO_MODE, BIT_FIFO_MODE_STREAM); + + LOG_DBG("FIFO_CONFIG (0x%x) 0x%x", REG_FIFO_CONFIG, 1 << 6); + res = icm42688_spi_single_write(&dev_cfg->spi, REG_FIFO_CONFIG, fifo_config); + + /* Config interrupt source to only be fifo wm/full */ + uint8_t int_source0 = BIT_FIFO_FULL_INT1_EN | BIT_FIFO_THS_INT1_EN; + + LOG_DBG("INT_SOURCE0 (0x%x) 0x%x", REG_INT_SOURCE0, int_source0); + res = icm42688_spi_single_write(&dev_cfg->spi, REG_INT_SOURCE0, int_source0); + if (res) { + return res; + } + } else { + LOG_INF("FIFO DISABLED"); + + /* No fifo mode so set data ready as interrupt source */ + uint8_t int_source0 = BIT_UI_DRDY_INT1_EN; + + LOG_DBG("INT_SOURCE0 (0x%x) 0x%x", REG_INT_SOURCE0, int_source0); + res = icm42688_spi_single_write(&dev_cfg->spi, REG_INT_SOURCE0, int_source0); + if (res) { + return res; + } } return res; } +int icm42688_safely_configure(const struct device *dev, struct icm42688_cfg *cfg) +{ + struct icm42688_dev_data *drv_data = dev->data; + int ret = icm42688_configure(dev, cfg); + + if (ret == 0) { + drv_data->cfg = *cfg; + } else { + ret = icm42688_configure(dev, &drv_data->cfg); + } + + return ret; +} + int icm42688_read_all(const struct device *dev, uint8_t data[14]) { const struct icm42688_dev_cfg *dev_cfg = dev->config; diff --git a/drivers/sensor/icm42688/icm42688_reg.h b/drivers/sensor/icm42688/icm42688_reg.h index 7127f5f1e8..dfc348c072 100644 --- a/drivers/sensor/icm42688/icm42688_reg.h +++ b/drivers/sensor/icm42688/icm42688_reg.h @@ -38,8 +38,8 @@ /* Bank 0 */ #define REG_DEVICE_CONFIG (REG_BANK0_OFFSET | 0x11) #define REG_DRIVE_CONFIG (REG_BANK0_OFFSET | 0x13) -#define REG_INT_CONFIG (REG_BANK0_OFFSET | 0x13) -#define REG_FIFO_CONFIG (REG_BANK0_OFFSET | 0x14) +#define REG_INT_CONFIG (REG_BANK0_OFFSET | 0x14) +#define REG_FIFO_CONFIG (REG_BANK0_OFFSET | 0x16) #define REG_TEMP_DATA1 (REG_BANK0_OFFSET | 0x1D) #define REG_TEMP_DATA0 (REG_BANK0_OFFSET | 0x1E) #define REG_ACCEL_DATA_X1 (REG_BANK0_OFFSET | 0x1F) @@ -118,6 +118,12 @@ /* Bank 2 */ +/* Bank 4 */ +#define REG_INT_SOURCE6 (REG_BANK4_OFFSET | 0x77) +#define REG_INT_SOURCE7 (REG_BANK4_OFFSET | 0x78) +#define REG_INT_SOURCE8 (REG_BANK4_OFFSET | 0x79) +#define REG_INT_SOURCE9 (REG_BANK4_OFFSET | 0x80) + /* Bank0 REG_DEVICE_CONFIG */ #define BIT_SOFT_RESET BIT(0) #define BIT_SPI_MODE BIT(4) @@ -245,6 +251,7 @@ #define BIT_ACCEL_ODR_500 0x0F /* Bank0 FIFO_CONFIG1 */ +#define BIT_FIFO_WM_GT_TH BIT(5) #define BIT_FIFO_HIRES_EN BIT(4) #define BIT_FIFO_TMST_FSYNC_EN BIT(3) #define BIT_FIFO_GYRO_EN BIT(2) @@ -260,6 +267,15 @@ #define BIT_FIFO_FULL_INT1_EN BIT(1) #define BIT_FIFO_UI_AGC_RDY_INT1_EN BIT(0) +/* Bank0 REG_FSYNC_CONFIG */ +#define MASK_FSYNC_UI_SEL GENMASK(6, 4) +#define BIT_FSYNC_UI + +/* Bank0 REG_INT_CONFIG1 */ +#define BIT_INT_TPULSE_DURATION BIT(6) +#define BIT_INT_TDEASSERT_DISABLE BIT(5) +#define BIT_INT_ASYNC_RESET BIT(4) + /* misc. defines */ #define WHO_AM_I_ICM42688 0x47 #define MIN_ACCEL_SENS_SHIFT 11