diff --git a/drivers/sensor/icm42688/icm42688.c b/drivers/sensor/icm42688/icm42688.c index ee92c30987ecfce..a6b4cd06c521978 100644 --- a/drivers/sensor/icm42688/icm42688.c +++ b/drivers/sensor/icm42688/icm42688.c @@ -157,13 +157,11 @@ static int icm42688_attr_set(const struct device *dev, enum sensor_channel chan, } break; case SENSOR_CHAN_ALL: - if (attr == SENSOR_ATTR_FIFO_WATERMARK) { - int64_t mval = sensor_value_to_micro(val); - - if (mval < 0 || mval > 1000000) { + if (attr == SENSOR_ATTR_BATCH_DURATION) { + if (val->val1 < 0) { return -EINVAL; } - new_config.fifo_wm = CLAMP(mval * 2048 / 1000000, 0, 2048); + new_config.batch_ticks = val->val1; } else { LOG_ERR("Unsupported attribute"); res = -EINVAL; @@ -217,6 +215,15 @@ static int icm42688_attr_get(const struct device *dev, enum sensor_channel chan, res = -EINVAL; } break; + case SENSOR_CHAN_ALL: + if (attr == SENSOR_ATTR_BATCH_DURATION) { + val->val1 = cfg->batch_ticks; + val->val2 = 0; + } else { + LOG_ERR("Unsupported attribute"); + res = -EINVAL; + } + break; default: LOG_ERR("Unsupported channel"); res = -EINVAL; @@ -272,7 +279,7 @@ int icm42688_init(const struct device *dev) data->cfg.gyro_odr = ICM42688_GYRO_ODR_1000; data->cfg.temp_dis = false; data->cfg.fifo_en = IS_ENABLED(CONFIG_ICM42688_STREAM); - data->cfg.fifo_wm = 0; + data->cfg.batch_ticks = 0; data->cfg.fifo_hires = 0; data->cfg.interrupt1_drdy = 0; data->cfg.interrupt1_fifo_ths = 0; diff --git a/drivers/sensor/icm42688/icm42688.h b/drivers/sensor/icm42688/icm42688.h index c4167e6a3b9cf4b..5ec470cff270c38 100644 --- a/drivers/sensor/icm42688/icm42688.h +++ b/drivers/sensor/icm42688/icm42688.h @@ -380,7 +380,7 @@ struct icm42688_cfg { /* TODO timestamp options */ bool fifo_en; - uint16_t fifo_wm; + int32_t batch_ticks; bool fifo_hires; /* TODO additional FIFO options */ diff --git a/drivers/sensor/icm42688/icm42688_common.c b/drivers/sensor/icm42688/icm42688_common.c index 9cb3037ed06ea3b..4df2f7870927665 100644 --- a/drivers/sensor/icm42688/icm42688_common.c +++ b/drivers/sensor/icm42688/icm42688_common.c @@ -62,6 +62,62 @@ int icm42688_reset(const struct device *dev) 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; @@ -210,7 +266,8 @@ int icm42688_configure(const struct device *dev, struct icm42688_cfg *cfg) } /* Set watermark and interrupt handling first */ - uint8_t fifo_wml = (cfg->fifo_wm) & 0xFF; + 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); @@ -219,7 +276,7 @@ int icm42688_configure(const struct device *dev, struct icm42688_cfg *cfg) return -EINVAL; } - uint8_t fifo_wmh = (cfg->fifo_wm >> 8) & 0x0F; + 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); diff --git a/drivers/sensor/sensor_shell.c b/drivers/sensor/sensor_shell.c index 5d41663c491db8a..bfeac4e452fd6ce 100644 --- a/drivers/sensor/sensor_shell.c +++ b/drivers/sensor/sensor_shell.c @@ -122,7 +122,7 @@ static const char *sensor_attribute_name[SENSOR_ATTR_COMMON_COUNT] = { [SENSOR_ATTR_FEATURE_MASK] = "feature_mask", [SENSOR_ATTR_ALERT] = "alert", [SENSOR_ATTR_FF_DUR] = "ff_dur", - [SENSOR_ATTR_FIFO_WATERMARK] = "fifo_wm", + [SENSOR_ATTR_BATCH_DURATION] = "batch_dur", }; /* Forward declaration */ diff --git a/include/zephyr/drivers/sensor.h b/include/zephyr/drivers/sensor.h index dd37f298779fe3e..480bd0d52c98a9b 100644 --- a/include/zephyr/drivers/sensor.h +++ b/include/zephyr/drivers/sensor.h @@ -334,8 +334,10 @@ enum sensor_attribute { * to the new sampling frequency. */ SENSOR_ATTR_FF_DUR, - /** Watermark % for the hardware fifo interrupt */ - SENSOR_ATTR_FIFO_WATERMARK, + + /** Hardware batch duration in ticks */ + SENSOR_ATTR_BATCH_DURATION, + /** * Number of all common sensor attributes. */