DNM: hacky example using the previously added APIs

samples/sensor/sensor_shell/prj.conf

@@ -4,4 +4,13 @@ CONFIG_SENSOR=y
 CONFIG_SENSOR_SHELL=y
 CONFIG_SENSOR_INFO=y
 
-CONFIG_LOG=y
+CONFIG_LOG=n
+CONFIG_SENSOR_LOG_LEVEL_INF=y
+CONFIG_ICM42688_TRIGGER_NONE=y
+CONFIG_ADC_ADS7052_INIT_PRIORITY=99
+
+CONFIG_RTIO=y
+CONFIG_RTIO_SYS_MEM_BLOCKS=y
+CONFIG_RTIO_CONSUME_SEM=y
+CONFIG_DSP=y
+CONFIG_CMSIS_DSP=y

samples/sensor/sensor_shell/src/main.c

@@ -6,11 +6,93 @@
 
 #include <stdlib.h>
 #include <zephyr/drivers/sensor.h>
+#include <zephyr/dsp/dsp.h>
 #include <zephyr/kernel.h>
 #include <zephyr/logging/log.h>
 
+#include <zephyr/rtio/rtio.h>
+#include <zephyr/rtio/rtio_executor_simple.h>
+
+#define LOOP_DURATION_MS 500
+#define BUSY_WAIT_DURATION_US 0
+#define DEDICATED_LOOP_THREAD 0
+
 LOG_MODULE_REGISTER(app);
 
+static const struct device *accel0_dev = DEVICE_DT_GET(DT_ALIAS(accel0));
+
+SENSOR_DT_READ_CONFIG(accel0, DT_ALIAS(accel0), SENSOR_CHAN_ACCEL_XYZ, SENSOR_CHAN_GYRO_XYZ);
+SENSOR_DT_READ_CONFIG(magn0, DT_ALIAS(magn0), SENSOR_CHAN_MAGN_XYZ);
+
+RTIO_EXECUTOR_SIMPLE_DEFINE(sensor_executor);
+RTIO_DEFINE_WITH_MEMPOOL(rtio_ctx, &sensor_executor, 1, 1, 2, 64, 4);
+
+static const struct sensor_decoder_api *accel0_decoder;
+static int num_callbacks, failed_reads, failed_timestamps, failed_frame_counts, failed_get_scale,
+	num_samples, num_requests;
+
+static void sensor_processing_callback(const struct device *sensor, int result, uint8_t *buf,
+				       uint32_t buf_len)
+{
+	int rc;
+
+	num_callbacks++;
+	/*printk("Reading from %p\n", (void*)buf);*/
+	if (result != 0) {
+		failed_reads++;
+		return;
+	}
+
+	uint64_t timestamp_ns;
+
+	rc = accel0_decoder->get_timestamp(buf, &timestamp_ns);
+	if (rc != 0) {
+		failed_timestamps++;
+		return;
+	}
+
+	uint16_t frame_count;
+
+	rc = accel0_decoder->get_frame_count(buf, &frame_count);
+	if (rc != 0) {
+		failed_frame_counts++;
+		return;
+	}
+
+	sensor_frame_iterator_t fit = {0};
+	sensor_channel_iterator_t cit = {0};
+	enum sensor_channel channels[6];
+	q31_t q[6];
+
+	while (true) {
+		uint32_t scales[2];
+		int count = accel0_decoder->decode(buf, &fit, &cit, channels, q, 6);
+
+		if (count <= 0) {
+			break;
+		}
+		rc = accel0_decoder->get_scale(buf, channels[0], &scales[0]);
+		rc |= accel0_decoder->get_scale(buf, channels[3], &scales[1]);
+		if (rc != 0) {
+			failed_get_scale++;
+			continue;
+		}
+
+		num_samples++;
+	}
+}
+
+#if DEDICATED_LOOP_THREAD
+static void sensor_processing_thread(void *p0, void *p1, void *p2)
+{
+	while (true) {
+		z_sensor_processing_loop(&rtio_ctx, sensor_processing_callback);
+	}
+}
+K_THREAD_DEFINE(sensor_processing_tid, 1024, sensor_processing_thread, NULL, NULL, NULL,
+		K_PRIO_COOP(10), 0, 0);
+#endif /* DEDICATED_LOOP_THREAD */
+
 enum sample_stats_state {
 	SAMPLE_STATS_STATE_UNINITIALIZED = 0,
 	SAMPLE_STATS_STATE_ENABLED,
@@ -24,6 +106,7 @@ struct sample_stats {
 	enum sample_stats_state state;
 };
 
+#if 0
 static void data_ready_trigger_handler(const struct device *sensor,
 				       const struct sensor_trigger *trigger)
 {
@@ -73,9 +156,11 @@ static void data_ready_trigger_handler(const struct device *sensor,
 		}
 	}
 }
+#endif
 
 int main(void)
 {
+#if 0
 	STRUCT_SECTION_FOREACH(sensor_info, sensor)
 	{
 		struct sensor_trigger trigger = {
@@ -84,5 +169,108 @@ int main(void)
 		};
 		sensor_trigger_set(sensor->dev, &trigger, data_ready_trigger_handler);
 	}
+#endif
+
+	int rc;
+	struct sensor_value value = {
+		.val1 = 32000,
+		.val2 = 0,
+	};
+	const struct device *icm42688 = DEVICE_DT_GET(DT_ALIAS(accel0));
+
+	rc = sensor_attr_set(icm42688, SENSOR_CHAN_ACCEL_XYZ, SENSOR_ATTR_SAMPLING_FREQUENCY,
+			     &value);
+	if (rc) {
+		printk("Error setting accel ODR\n");
+		return -1;
+	}
+	rc = sensor_attr_set(icm42688, SENSOR_CHAN_GYRO_XYZ, SENSOR_ATTR_SAMPLING_FREQUENCY,
+			     &value);
+	if (rc) {
+		printk("Error setting gyro ODR\n");
+		return -1;
+	}
+	rc = sensor_get_decoder(icm42688, &accel0_decoder);
+	if (rc) {
+		printk("Error getting decoder\n");
+		return -1;
+	}
+
+	uint64_t start_ts = k_uptime_get();
+	uint64_t end_ts = start_ts;
+
+	do {
+		rc = sensor_read(&accel0, &rtio_ctx);
+		if (rc == 0) {
+			num_requests++;
+			end_ts = k_uptime_get();
+		} else {
+			printk("Failed to call sensor_read (rc=%d)\n", rc);
+		}
+		if (BUSY_WAIT_DURATION_US > 0) {
+			k_busy_wait(BUSY_WAIT_DURATION_US);
+		}
+		z_sensor_processing_loop(&rtio_ctx, sensor_processing_callback);
+	} while (end_ts - start_ts < LOOP_DURATION_MS);
+
+	k_msleep(1000);
+
+	uint64_t duration = end_ts - start_ts;
+	printk("Statistics:\n");
+	printk("Duration: %" PRIu64 ".%03" PRIu64 "\n", duration / 1000, duration % 1000);
+	printk("Num Requests: %d\n", num_requests);
+	printk("Sampling frequency: %" PRIu64 "Hz (ideal: 32000Hz)\n",
+	       (UINT64_C(1000000) * num_requests) / (duration * 1000));
+	printk("Reading frequency: %" PRIu64 "Hz\n",
+	       (UINT64_C(1000000) * num_samples) / (duration * 1000));
+	printk("Num Callbacks: %d\n", num_callbacks);
+	printk("Num Samples: %d\n", num_samples);
+	int drop_rate = (failed_reads * 100000) / num_callbacks;
+	printk("Drop rate: %d.%02d\n", drop_rate / 1000, drop_rate % 1000);
+	printk("Num Failed Reads: %d\n", failed_reads);
+	printk("Num Failed Timestamps: %d\n", failed_timestamps);
+	printk("Num Failed Frame Counts: %d\n", failed_frame_counts);
+	printk("Num Failed Get Scale: %d\n", failed_get_scale);
+	printk("Completion overrun count %d\n", (uint32_t)atomic_get(&rtio_ctx.xcqcnt));
+
+	num_requests = 0;
+	failed_reads = 0;
+	num_samples = 0;
+
+	start_ts = k_uptime_get();
+	end_ts = start_ts;
+	do {
+		int rc = sensor_sample_fetch(accel0_dev);
+
+		num_requests++;
+		if (rc != 0) {
+			failed_reads++;
+		} else {
+			struct sensor_value values[3];
+
+			rc = sensor_channel_get(accel0_dev, SENSOR_CHAN_ACCEL_XYZ, values);
+			rc |= sensor_channel_get(accel0_dev, SENSOR_CHAN_GYRO_XYZ, values);
+
+			if (rc == 0) {
+				num_samples++;
+			}
+		}
+		end_ts = k_uptime_get();
+		if (BUSY_WAIT_DURATION_US > 0) {
+			k_busy_wait(BUSY_WAIT_DURATION_US);
+		}
+	} while (end_ts - start_ts < LOOP_DURATION_MS);
+
+	duration = end_ts - start_ts;
+	printk("\n\n*************************\n");
+	printk("Legacy Statistics:\n");
+	printk("Duration: %" PRIu64 ".%03" PRIu64 "\n", duration / 1000, duration % 1000);
+	printk("Sampling frequency: %" PRIu64 "Hz (ideal: 32000Hz)\n",
+	       (UINT64_C(1000000) * num_requests) / (duration * 1000));
+	printk("Reading frequency: %" PRIu64 "Hz\n",
+	       (UINT64_C(1000000) * num_samples) / (duration * 1000));
+	printk("Num Requests: %d\n", num_requests);
+	printk("Num Samples: %d\n", num_samples);
+
 	return 0;
 }