sensor_shell: Update to new sensor_read API

Update the sensor shell logic to use the new sensor_read() APIs and
make triggers an option of the sensor_shell sample (this avoids the
trigger stealing the interrupt status from one-shot reads).

Signed-off-by: Yuval Peress <peress@google.com>

drivers/sensor/sensor_shell.c

@@ -4,12 +4,14 @@
  * SPDX-License-Identifier: Apache-2.0
  */
 
-#include <zephyr/shell/shell.h>
+#include <ctype.h>
 #include <stdlib.h>
 #include <string.h>
-#include <ctype.h>
+
 #include <zephyr/device.h>
 #include <zephyr/drivers/sensor.h>
+#include <zephyr/rtio/rtio.h>
+#include <zephyr/shell/shell.h>
 
 #define SENSOR_GET_HELP                                                                            \
 	"Get sensor data. Channel names are optional. All channels are read "                      \
@@ -113,6 +115,19 @@ enum dynamic_command_context {
 
 static enum dynamic_command_context current_cmd_ctx = NONE;
 
+/* Crate a single common config for one-shot reading */
+static enum sensor_channel iodev_sensor_shell_channels[SENSOR_CHAN_ALL];
+static struct sensor_read_config iodev_sensor_shell_read_config = {
+	.sensor = NULL,
+	.channels = iodev_sensor_shell_channels,
+	.count = 0,
+	.max = ARRAY_SIZE(iodev_sensor_shell_channels),
+};
+RTIO_IODEV_DEFINE(iodev_sensor_shell_read, &__sensor_iodev_api, &iodev_sensor_shell_read_config);
+
+/* Create the RTIO context to service the reading */
+RTIO_DEFINE_WITH_MEMPOOL(sensor_read_rtio, 8, 8, 32, 64, 4);
+
 static int parse_named_int(const char *name, const char *heystack[], size_t count)
 {
 	char *endptr;
@@ -175,43 +190,71 @@ static int parse_sensor_value(const char *val_str, struct sensor_value *out)
 	return 0;
 }
 
-static int handle_channel_by_name(const struct shell *shell_ptr, const struct device *dev,
-				  const char *channel_name)
+struct sensor_shell_processing_context {
+	const struct device *dev;
+	const struct shell *sh;
+};
+
+static void sensor_shell_processing_callback(int result, uint8_t *buf, uint32_t buf_len,
+					     void *userdata)
 {
-	struct sensor_value value[3];
-	int err;
-	const int i =
-		parse_named_int(channel_name, sensor_channel_name, ARRAY_SIZE(sensor_channel_name));
+	struct sensor_shell_processing_context *ctx = userdata;
+	const struct sensor_decoder_api *decoder;
+	sensor_frame_iterator_t fit = {0};
+	sensor_channel_iterator_t cit = {0};
+	uint64_t timestamp;
+	enum sensor_channel channel;
+	q31_t q;
+	int rc;
 
-	if (i < 0) {
-		shell_error(shell_ptr, "Channel not supported (%s)", channel_name);
-		return i;
+	ARG_UNUSED(buf_len);
+
+	if (result < 0) {
+		shell_error(ctx->sh, "Read failed");
+		return;
 	}
 
-	err = sensor_channel_get(dev, i, value);
-	if (err < 0) {
-		return err;
+	rc = sensor_get_decoder(ctx->dev, &decoder);
+	if (rc != 0) {
+		shell_error(ctx->sh, "Failed to get decoder for '%s'", ctx->dev->name);
+		return;
 	}
 
-	if (i >= ARRAY_SIZE(sensor_channel_name)) {
-		shell_print(shell_ptr, "channel idx=%d value = %10.6f", i,
-			    sensor_value_to_double(&value[0]));
-	} else if (i != SENSOR_CHAN_ACCEL_XYZ && i != SENSOR_CHAN_GYRO_XYZ &&
-		   i != SENSOR_CHAN_MAGN_XYZ) {
-		shell_print(shell_ptr, "channel idx=%d %s = %10.6f", i, sensor_channel_name[i],
-			    sensor_value_to_double(&value[0]));
-	} else {
-		/* clang-format off */
-		shell_print(shell_ptr,
-			"channel idx=%d %s x = %10.6f y = %10.6f z = %10.6f",
-			i, sensor_channel_name[i],
-			sensor_value_to_double(&value[0]),
-			sensor_value_to_double(&value[1]),
-			sensor_value_to_double(&value[2]));
-		/* clang-format on */
+	rc = decoder->get_timestamp(buf, &timestamp);
+	if (rc != 0) {
+		shell_error(ctx->sh, "Failed to get fetch timestamp for '%s'", ctx->dev->name);
+		return;
 	}
+	shell_print(ctx->sh, "Got samples at %" PRIu64 " ns", timestamp);
 
-	return 0;
+	while (decoder->decode(buf, &fit, &cit, &channel, &q, 1) > 0) {
+		int8_t shift;
+
+		rc = decoder->get_shift(buf, channel, &shift);
+		if (rc != 0) {
+			shell_error(ctx->sh, "Failed to get bitshift for channel %d", channel);
+			continue;
+		}
+
+		int64_t scaled_value = (int64_t)q << shift;
+		bool is_negative = scaled_value < 0;
+		int numerator;
+		int denominator;
+
+		scaled_value = llabs(scaled_value);
+		numerator = (int)FIELD_GET(GENMASK64(31 + shift, 31), scaled_value);
+		denominator =
+			(int)((FIELD_GET(GENMASK64(30, 0), scaled_value) * 1000000) / INT32_MAX);
+
+		if (channel >= ARRAY_SIZE(sensor_channel_name)) {
+			shell_print(ctx->sh, "channel idx=%d value=%s%d.%06d", channel,
+				    is_negative ? "-" : "", numerator, denominator);
+		} else {
+			shell_print(ctx->sh, "channel idx=%d %s value=%s%d.%06d", channel,
+				    sensor_channel_name[channel], is_negative ? "-" : "", numerator,
+				    denominator);
+		}
+	}
 }
 
 static int cmd_get_sensor(const struct shell *sh, size_t argc, char *argv[])
@@ -225,27 +268,50 @@ static int cmd_get_sensor(const struct shell *sh, size_t argc, char *argv[])
 		return -ENODEV;
 	}
 
-	err = sensor_sample_fetch(dev);
-	if (err < 0) {
-		shell_error(sh, "Failed to read sensor: %d", err);
-	}
-
 	if (argc == 2) {
 		/* read all channels */
-		for (int i = 0; i < ARRAY_SIZE(sensor_channel_name); i++) {
-			if (sensor_channel_name[i]) {
-				handle_channel_by_name(sh, dev, sensor_channel_name[i]);
+		int count = 0;
+
+		for (int i = 0; i < ARRAY_SIZE(iodev_sensor_shell_channels); ++i) {
+			if (SENSOR_CHANNEL_3_AXIS(i)) {
+				continue;
 			}
+			iodev_sensor_shell_channels[count++] = i;
 		}
+		iodev_sensor_shell_read_config.count = count;
 	} else {
-		for (int i = 2; i < argc; i++) {
-			err = handle_channel_by_name(sh, dev, argv[i]);
-			if (err < 0) {
+		/* read specific channels */
+		iodev_sensor_shell_read_config.count = 0;
+		for (int i = 2; i < argc; ++i) {
+			int chan = parse_named_int(argv[i], sensor_channel_name,
+						   ARRAY_SIZE(sensor_channel_name));
+
+			if (chan < 0) {
 				shell_error(sh, "Failed to read channel (%s)", argv[i]);
+				continue;
 			}
+			iodev_sensor_shell_channels[iodev_sensor_shell_read_config.count++] =
+				chan;
 		}
 	}
 
+	/* TODO(yperess) use sensor_reconfigure_read_iodev? */
+	if (iodev_sensor_shell_read_config.count == 0) {
+		shell_error(sh, "No channels to read, bailing");
+		return -EINVAL;
+	}
+	iodev_sensor_shell_read_config.sensor = dev;
+
+	struct sensor_shell_processing_context ctx = {
+		.dev = dev,
+		.sh = sh,
+	};
+	err = sensor_read(&iodev_sensor_shell_read, &sensor_read_rtio, &ctx);
+	if (err < 0) {
+		shell_error(sh, "Failed to read sensor: %d", err);
+	}
+	z_sensor_processing_loop(&sensor_read_rtio, sensor_shell_processing_callback);
+
 	return 0;
 }
 

samples/sensor/sensor_shell/CMakeLists.txt

@@ -5,5 +5,7 @@ cmake_minimum_required(VERSION 3.20.0)
 find_package(Zephyr REQUIRED HINTS $ENV{ZEPHYR_BASE})
 project(sensor_shell)
 
-FILE(GLOB app_sources src/*.c)
-target_sources(app PRIVATE ${app_sources})
+target_sources(app PRIVATE src/main.c)
+target_sources_ifdef(CONFIG_INIT_TRIG_DATA_READY app PRIVATE src/trigger.c)
+
+target_include_directories(app PRIVATE include)

samples/sensor/sensor_shell/Kconfig

@@ -9,3 +9,9 @@ config SAMPLE_PRINT_TIMEOUT_MS
 	  interrupt handler will collect data.
 
 source "Kconfig.zephyr"
+
+config INIT_TRIG_DATA_READY
+	bool "Register data ready triggers for all sensors on start"
+	help
+	  When the application starts, automatically register data ready trigger
+	  listeners to all available sensors.

samples/sensor/sensor_shell/include/trigger.h

@@ -0,0 +1,16 @@
+/*
+ * Copyright (c) 2023 Google LLC
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#ifndef ZEPHYR_SAMPLES_SENSOR_SENSOR_SHELL_INCLUDE_TRIGGER_H
+#define ZEPHYR_SAMPLES_SENSOR_SENSOR_SHELL_INCLUDE_TRIGGER_H
+
+#include <zephyr/device.h>
+#include <zephyr/drivers/sensor.h>
+
+void sensor_shell_data_ready_trigger_handler(const struct device *sensor,
+					     const struct sensor_trigger *trigger);
+
+#endif /* ZEPHYR_SAMPLES_SENSOR_SENSOR_SHELL_INCLUDE_TRIGGER_H */

samples/sensor/sensor_shell/prj.conf

@@ -5,3 +5,4 @@ CONFIG_SENSOR_SHELL=y
 CONFIG_SENSOR_INFO=y
 
 CONFIG_LOG=y
+CONFIG_RTIO_CONSUME_SEM=y

samples/sensor/sensor_shell/src/main.c

@@ -9,80 +9,22 @@
 #include <zephyr/kernel.h>
 #include <zephyr/logging/log.h>
 
-LOG_MODULE_REGISTER(app);
-
-enum sample_stats_state {
-	SAMPLE_STATS_STATE_UNINITIALIZED = 0,
-	SAMPLE_STATS_STATE_ENABLED,
-	SAMPLE_STATS_STATE_DISABLED,
-};
-
-struct sample_stats {
-	int64_t accumulator;
-	uint32_t count;
-	uint64_t sample_window_start;
-	enum sample_stats_state state;
-};
-
-static void data_ready_trigger_handler(const struct device *sensor,
-				       const struct sensor_trigger *trigger)
-{
-	static struct sample_stats stats[SENSOR_CHAN_ALL];
-	const int64_t now = k_uptime_get();
-	struct sensor_value value;
-
-	if (sensor_sample_fetch(sensor)) {
-		LOG_ERR("Failed to fetch samples on data ready handler");
-	}
-	for (int i = 0; i < SENSOR_CHAN_ALL; ++i) {
-		int rc;
-
-		/* Skip disabled channels */
-		if (stats[i].state == SAMPLE_STATS_STATE_DISABLED) {
-			continue;
-		}
-		/* Skip 3 axis channels */
-		if (i == SENSOR_CHAN_ACCEL_XYZ || i == SENSOR_CHAN_GYRO_XYZ ||
-		    i == SENSOR_CHAN_MAGN_XYZ) {
-			continue;
-		}
+#include "trigger.h"
 
-		rc = sensor_channel_get(sensor, i, &value);
-		if (rc == -ENOTSUP && stats[i].state == SAMPLE_STATS_STATE_UNINITIALIZED) {
-			/* Stop reading this channel if the driver told us it's not supported. */
-			stats[i].state = SAMPLE_STATS_STATE_DISABLED;
-		}
-		if (rc != 0) {
-			/* Skip on any error. */
-			continue;
-		}
-		/* Do something with the data */
-		stats[i].accumulator += value.val1 * INT64_C(1000000) + value.val2;
-		if (stats[i].count++ == 0) {
-			stats[i].sample_window_start = now;
-		} else if (now > stats[i].sample_window_start + CONFIG_SAMPLE_PRINT_TIMEOUT_MS) {
-			int64_t micro_value = stats[i].accumulator / stats[i].count;
-
-			value.val1 = micro_value / 1000000;
-			value.val2 = (int32_t)llabs(micro_value - (value.val1 * 1000000));
-			LOG_INF("chan=%d, num_samples=%u, data=%d.%06d", i, stats[i].count,
-				value.val1, value.val2);
-
-			stats[i].accumulator = 0;
-			stats[i].count = 0;
-		}
-	}
-}
+LOG_MODULE_REGISTER(app);
 
 int main(void)
 {
-	STRUCT_SECTION_FOREACH(sensor_info, sensor)
-	{
-		struct sensor_trigger trigger = {
-			.chan = SENSOR_CHAN_ALL,
-			.type = SENSOR_TRIG_DATA_READY,
-		};
-		sensor_trigger_set(sensor->dev, &trigger, data_ready_trigger_handler);
+	if (IS_ENABLED(CONFIG_INIT_TRIG_DATA_READY)) {
+		STRUCT_SECTION_FOREACH(sensor_info, sensor)
+		{
+			struct sensor_trigger trigger = {
+				.chan = SENSOR_CHAN_ALL,
+				.type = SENSOR_TRIG_DATA_READY,
+			};
+			sensor_trigger_set(sensor->dev, &trigger,
+					   sensor_shell_data_ready_trigger_handler);
+		}
 	}
 	return 0;
 }