341 lines
9.4 KiB
C
341 lines
9.4 KiB
C
/*
|
|
* Copyright (c) 2022 Intel Corporation
|
|
* Copyright (c) 2022 Esco Medical ApS
|
|
* Copyright (c) 2020 TDK Invensense
|
|
*
|
|
* SPDX-License-Identifier: Apache-2.0
|
|
*/
|
|
|
|
#include <zephyr/drivers/sensor.h>
|
|
#include <zephyr/drivers/spi.h>
|
|
#include <zephyr/sys/byteorder.h>
|
|
#include "icm42688.h"
|
|
#include "icm42688_reg.h"
|
|
#include "icm42688_spi.h"
|
|
#include "icm42688_trigger.h"
|
|
|
|
#include <zephyr/logging/log.h>
|
|
LOG_MODULE_REGISTER(ICM42688_LL, CONFIG_SENSOR_LOG_LEVEL);
|
|
|
|
int icm42688_reset(const struct device *dev)
|
|
{
|
|
int res;
|
|
uint8_t value;
|
|
const struct icm42688_dev_cfg *dev_cfg = dev->config;
|
|
|
|
/* start up time for register read/write after POR is 1ms and supply ramp time is 3ms */
|
|
k_msleep(3);
|
|
|
|
/* perform a soft reset to ensure a clean slate, reset bit will auto-clear */
|
|
res = icm42688_spi_single_write(&dev_cfg->spi, REG_DEVICE_CONFIG, BIT_SOFT_RESET);
|
|
|
|
if (res) {
|
|
LOG_ERR("write REG_SIGNAL_PATH_RESET failed");
|
|
return res;
|
|
}
|
|
|
|
/* wait for soft reset to take effect */
|
|
k_msleep(SOFT_RESET_TIME_MS);
|
|
|
|
/* clear reset done int flag */
|
|
res = icm42688_spi_read(&dev_cfg->spi, REG_INT_STATUS, &value, 1);
|
|
|
|
if (res) {
|
|
return res;
|
|
}
|
|
|
|
if (FIELD_GET(BIT_INT_STATUS_RESET_DONE, value) != 1) {
|
|
LOG_ERR("unexpected RESET_DONE value, %i", value);
|
|
return -EINVAL;
|
|
}
|
|
|
|
res = icm42688_spi_read(&dev_cfg->spi, REG_WHO_AM_I, &value, 1);
|
|
if (res) {
|
|
return res;
|
|
}
|
|
|
|
if (value != WHO_AM_I_ICM42688) {
|
|
LOG_ERR("invalid WHO_AM_I value, was %i but expected %i", value, WHO_AM_I_ICM42688);
|
|
return -EINVAL;
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
static uint16_t icm42688_compute_fifo_wm(const struct icm42688_cfg *cfg)
|
|
{
|
|
const bool accel_enabled = cfg->accel_mode != ICM42688_ACCEL_OFF;
|
|
const bool gyro_enabled = cfg->gyro_mode != ICM42688_GYRO_OFF;
|
|
const int pkt_size = cfg->fifo_hires ? 20 : (accel_enabled && gyro_enabled ? 16 : 8);
|
|
int accel_modr = 0;
|
|
int gyro_modr = 0;
|
|
int64_t modr;
|
|
|
|
if (cfg->batch_ticks == 0 || (!accel_enabled && !gyro_enabled)) {
|
|
return 0;
|
|
}
|
|
|
|
if (accel_enabled) {
|
|
struct sensor_value val = {0};
|
|
|
|
icm42688_accel_reg_to_hz(cfg->accel_odr, &val);
|
|
accel_modr = sensor_value_to_micro(&val) / 1000;
|
|
}
|
|
if (gyro_enabled) {
|
|
struct sensor_value val = {0};
|
|
|
|
icm42688_gyro_reg_to_odr(cfg->gyro_odr, &val);
|
|
gyro_modr = sensor_value_to_micro(&val) / 1000;
|
|
}
|
|
|
|
if (accel_modr == 0) {
|
|
modr = gyro_modr;
|
|
} else if (gyro_modr == 0) {
|
|
modr = accel_modr;
|
|
} else {
|
|
/* Need to find the least common multiplier (LCM) */
|
|
int n1 = accel_modr;
|
|
int n2 = gyro_modr;
|
|
|
|
while (n1 != n2) {
|
|
if (n1 > n2) {
|
|
n1 -= n2;
|
|
} else {
|
|
n2 -= n1;
|
|
}
|
|
}
|
|
LOG_DBG("GCD=%d", n1);
|
|
modr = ((int64_t)accel_modr * (int64_t)gyro_modr) / n1;
|
|
}
|
|
/* At this point we have 'modr' as mHz which is 1 / msec. */
|
|
|
|
/* Convert 'modr' to bytes * batch_ticks / msec */
|
|
modr *= (int64_t)cfg->batch_ticks * pkt_size;
|
|
|
|
/* 'modr' = byte_ticks_per_msec / kticks_per_sec */
|
|
modr = DIV_ROUND_UP(modr, CONFIG_SYS_CLOCK_TICKS_PER_SEC * INT64_C(1000));
|
|
|
|
return (uint16_t)MIN(modr, 0x7ff);
|
|
}
|
|
|
|
int icm42688_configure(const struct device *dev, struct icm42688_cfg *cfg)
|
|
{
|
|
struct icm42688_dev_data *dev_data = dev->data;
|
|
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,
|
|
FIELD_PREP(MASK_FIFO_MODE, BIT_FIFO_MODE_BYPASS));
|
|
|
|
if (res != 0) {
|
|
LOG_ERR("Error writing FIFO_CONFIG");
|
|
return -EINVAL;
|
|
}
|
|
|
|
res = icm42688_spi_single_write(&dev_cfg->spi, REG_SIGNAL_PATH_RESET,
|
|
FIELD_PREP(BIT_FIFO_FLUSH, 1));
|
|
|
|
if (res != 0) {
|
|
LOG_ERR("Error flushing fifo");
|
|
return -EINVAL;
|
|
}
|
|
}
|
|
|
|
/* TODO maybe do the next few steps intelligently by checking current config */
|
|
|
|
/* Power management to set gyro/accel modes */
|
|
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;
|
|
}
|
|
|
|
/* Need to wait at least 200us before updating more registers
|
|
* see datasheet 14.36
|
|
*/
|
|
k_busy_wait(250);
|
|
|
|
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;
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
/*
|
|
* Accelerometer sensor need at least 10ms startup time
|
|
* Gyroscope sensor need at least 30ms startup time
|
|
*/
|
|
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) */
|
|
if (IS_ENABLED(CONFIG_ICM42688_TRIGGER)) {
|
|
res = icm42688_trigger_enable_interrupt(dev, cfg);
|
|
} else {
|
|
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->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) {
|
|
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 */
|
|
uint16_t fifo_wm = icm42688_compute_fifo_wm(cfg);
|
|
uint8_t fifo_wml = 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;
|
|
}
|
|
|
|
uint8_t fifo_wmh = (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;
|
|
}
|
|
|
|
/* Begin streaming */
|
|
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;
|
|
int res;
|
|
|
|
res = icm42688_spi_read(&dev_cfg->spi, REG_TEMP_DATA1, data, 14);
|
|
|
|
return res;
|
|
}
|