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 <[email protected]>
zephyrproject-rtos · Nov 10, 2023 · 3c6e66e · 3c6e66e
1 parent 1326c7c
commit 3c6e66e
Show file tree

Hide file tree

Showing 5 changed files with 78 additions and 12 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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
@@ -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.
 	 */