From 3c6e66e5b576d774a0357dc2af61d6c4eb4f3307 Mon Sep 17 00:00:00 2001 From: Yuval Peress Date: Tue, 10 Oct 2023 01:59:04 -0600 Subject: [PATCH] sensors: convert fifo_wm to batch_dur Having a % FIFO watermark isn't very useful as it doesn't convey how long the SoC can sleep (or do other work) while batching sensor data. Convert the attribute to a batch duration using ticks. Currently the ticks are in system ticks, but eventually when an external clock is attached to the sensor it will be in the external clock's ticks. Signed-off-by: Yuval Peress --- drivers/sensor/icm42688/icm42688.c | 19 ++++--- drivers/sensor/icm42688/icm42688.h | 2 +- drivers/sensor/icm42688/icm42688_common.c | 61 ++++++++++++++++++++++- drivers/sensor/sensor_shell.c | 2 +- include/zephyr/drivers/sensor.h | 6 ++- 5 files changed, 78 insertions(+), 12 deletions(-) diff --git a/drivers/sensor/icm42688/icm42688.c b/drivers/sensor/icm42688/icm42688.c index ee92c30987ecfc..a6b4cd06c52197 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 c4167e6a3b9cf4..5ec470cff270c3 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 9cb3037ed06ea3..4df2f787092766 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 5d41663c491db8..bfeac4e452fd6c 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 80fda1b163483e..e652c3a2040563 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. */