samples: boards: TDK Robokit1 Sensor Sample

Provides a sample demonstrating the fast sensor on a tdk robokit1 used
with and without RTIO.

With RTIO the sample may be built with SPI_RTIO enabled or disabled to
get a sense of the cost of a filler thread when SPI does not support RTIO
as well.

Signed-off-by: Tom Burdick <[email protected]>

Author: teburd

samples/boards/tdk_robokit1/CMakeLists.txt

@@ -0,0 +1,11 @@
+# Copyright (c) 2022 Intel Corporation
+#
+# SPDX-License-Identifier: Apache-2.0
+#
+cmake_minimum_required(VERSION 3.20.0)
+
+find_package(Zephyr REQUIRED HINTS $ENV{ZEPHYR_BASE})
+project(tdk_robokit1)
+
+target_include_directories(app PRIVATE $ENV{ZEPHYR_BASE}/drivers/sensor/icm42688)
+target_sources(app PRIVATE src/main.c)

samples/boards/tdk_robokit1/README.rst

@@ -0,0 +1,22 @@
+.. _tdk_robokit1 sensors:
+
+TDK Robokit1
+############
+
+Overview
+********
+
+This sample provides an example of how to read sensors data
+from the TDK RoboKit1 board using polling and RTIO.
+
+This sample enables all sensors of TDK RoboKit1 board, and then
+periodically reads and displays data on the console from the following
+sensors:
+
+- ICM42688: 6-Axis acceleration and angular velocity
+
+Requirements
+************
+
+The application requires a TDK RoboKit1 board connected to the PC
+through USB.

samples/boards/tdk_robokit1/prj.conf

@@ -0,0 +1,37 @@
+CONFIG_ARM_MPU=y # Seemingly not working
+CONFIG_FPU=n
+CONFIG_CBPRINTF_FP_SUPPORT=n
+CONFIG_ASSERT=y
+CONFIG_STACK_SENTINEL=y
+CONFIG_ENTROPY_GENERATOR=y
+CONFIG_STACK_CANARIES=y
+CONFIG_LOG=y # causes mpu fault?
+CONFIG_LOG_PRINTK=y
+CONFIG_LOG_MODE_DEFERRED=y
+CONFIG_SPI=y
+CONFIG_I2C=y
+CONFIG_GPIO=y
+CONFIG_LOG_PROCESS_THREAD_STACK_SIZE=8192
+CONFIG_MAIN_STACK_SIZE=2048
+CONFIG_ISR_STACK_SIZE=2048
+CONFIG_ICM42688_THREAD_STACK_SIZE=768
+
+# config sensors
+CONFIG_DMA=y
+CONFIG_DMA_LOG_LEVEL_INF=y
+CONFIG_SENSOR=y
+CONFIG_SENSOR_LOG_LEVEL_DBG=y
+CONFIG_SPI_LOG_LEVEL_DBG=y
+CONFIG_ICM42688_RTIO=y
+CONFIG_RTIO=y
+CONFIG_SPI_SAM_DMA=y
+#CONFIG_RTIO_SUBMIT_SEM=n
+CONFIG_RTIO_CONSUME_SEM=y
+
+
+# SystemView Tracing
+CONFIG_TRACING=y
+CONFIG_TRACING_TIMER=y
+CONFIG_TRACING_SYSCALL=n
+CONFIG_SEGGER_SYSTEMVIEW=y
+CONFIG_SEGGER_SYSVIEW_RTT_BUFFER_SIZE=65536

samples/boards/tdk_robokit1/sample.yaml

@@ -0,0 +1,8 @@
+sample:
+  description: TDK RoboKit1 Test App for Sensors
+  name: tdk_robokit1
+
+tests:
+  sample.boards.tdk_robokit1:
+    platform_allow: tdk_robokit1
+    tags: rtio sensor

samples/boards/tdk_robokit1/src/main.c

@@ -0,0 +1,190 @@
+/*
+ * Copyright (c) 2022 Intel Corporation
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#include <zephyr/kernel.h>
+#include <zephyr/drivers/sensor.h>
+#include <zephyr/rtio/rtio.h>
+#include <zephyr/rtio/rtio_executor_simple.h>
+
+#include <stdint.h>
+#include <stdio.h>
+
+#include <zephyr/logging/log.h>
+LOG_MODULE_REGISTER(TDK_ROBOKIT1, CONFIG_SENSOR_LOG_LEVEL);
+
+/* Power of 2 number of stream buffers */
+#define N_BUFS 4
+
+RTIO_EXECUTOR_SIMPLE_DEFINE(r_exec);
+
+/* Show no over/under flows with a 4 buffer queue */
+RTIO_DEFINE(r4, (struct rtio_executor *)&r_exec, N_BUFS, N_BUFS);
+
+/* Show over/underflows with a single buffer queue */
+RTIO_DEFINE(r1, (struct rtio_executor *)&r_exec, 1, 1);
+
+#define FIFO_ITERS 4096
+
+struct fifo_header {
+	uint8_t int_status;
+	uint16_t gyro_odr: 4;
+	uint16_t accel_odr: 4;
+	uint16_t gyro_fs: 3;
+	uint16_t accel_fs: 3;
+	uint16_t packet_format: 2;
+} __packed;
+
+/* The fifo size (2048) bytes plus 3 for the header */
+#define ICM42688_FIFO_BUF_LEN 2051
+static uint8_t icm42688_fifo_bufs[N_BUFS][ICM42688_FIFO_BUF_LEN];
+
+
+void fifo_stream(const struct device *icm42688, struct rtio *r, uint32_t n_bufs)
+{
+	struct rtio_iodev *iodev;
+	struct rtio_sqe *sqe;
+
+	LOG_INF("FIFO with RTIO context %p, context size %u", r, sizeof(*r));
+
+	/* obtain reference the stream */
+	(void)sensor_fifo_iodev(icm42688, &iodev);
+
+	LOG_INF("Setting up RX requests");
+
+	__ASSERT_NO_MSG(iodev != NULL);
+
+	/* Feed initial read requests */
+	for (int i = 0; i < n_bufs; i++) {
+		sqe = rtio_spsc_acquire(r->sq);
+		rtio_sqe_prep_read(sqe, iodev, 0, &icm42688_fifo_bufs[i][0], ICM42688_FIFO_BUF_LEN,
+				   (void *)(uintptr_t)i);
+		rtio_spsc_produce(r->sq);
+	}
+
+	/* Submits requests */
+	rtio_submit(r, 0);
+
+	/* Setup requests, starting stream */
+	LOG_INF("Polling RX Requests");
+
+	/* Enable the fifo automatic triggering (via gpio) */
+	sensor_fifo_start(icm42688);
+
+	uint32_t overflows = 0;
+
+	/* Now poll for ready fifo buffers for a little bit, at 32KHz sampling
+	 * and a 1024 buffer read with 16 bytes a sample, the fifo holds
+	 * 64 samples. At 32KHz it triggers every 2ms! this means
+	 * Every buffer has ~2ms to process or we lose the next sample.
+	 *
+	 * Every 8th buffer we busy wait just over 2ms to show how no data
+	 * is lost even with some variable latency involved. This could
+	 * come from data processing or attempting to transport the data
+	 * to another device.
+	 *
+	 * From the sensor a lack of buffer to read into is an underflow
+	 * and from the application perspective being unable to keep up
+	 * is an overflow.
+	 */
+	for (int i = 0; i < FIFO_ITERS; i++) {
+		struct rtio_cqe *cqe = rtio_cqe_consume_block(r);
+		int32_t result = cqe->result;
+		uintptr_t buf_idx = (uintptr_t)cqe->userdata;
+		uint8_t *buf = &icm42688_fifo_bufs[buf_idx][0];
+
+		rtio_spsc_release(r->cq);
+
+		/* The first byte is the interrupt status and can be
+		 * checked for a FIFO FULL signifying an overflow
+		 */
+		struct fifo_header *hdr = (void *)&buf[0];
+
+		if (hdr->int_status & BIT(1)) {
+			overflows++;
+		}
+
+
+		if (i % 128 == 0) {
+			printk("Slow mode on iteration %d, underflows (sensor overflows) %u, buf "
+			       "%lu, data int status %x, result %d\n",
+			       i, overflows, buf_idx, buf[0], result);
+			k_busy_wait(4000);
+		}
+
+		/* Now to recycle the buffer by putting it back in the queue */
+		struct rtio_sqe *sqe = rtio_spsc_acquire(r->sq);
+
+		__ASSERT_NO_MSG(sqe != NULL);
+		rtio_sqe_prep_read(sqe, iodev, 0, &icm42688_fifo_bufs[buf_idx][0],
+				   ICM42688_FIFO_BUF_LEN, (void *)(uintptr_t)buf_idx);
+		rtio_spsc_produce(r->sq);
+
+		rtio_submit(r, 0);
+
+	}
+
+	LOG_INF("Checking In, Sensor Overflows %u", overflows);
+
+	/* All done streaming */
+	sensor_fifo_stop(icm42688);
+
+	LOG_INF("DONE! FIFO should be DISABLED");
+}
+
+void main(void)
+{
+	const struct device *icm42688 = DEVICE_DT_GET_ONE(invensense_icm42688);
+	struct sensor_value accel[3];
+	struct sensor_value gyro[3];
+	struct sensor_value temp;
+
+
+	LOG_INF("TDK RoboKit1 Sample");
+
+
+	if (!device_is_ready(icm42688)) {
+		LOG_INF("%s: device not ready.", icm42688->name);
+		return;
+	}
+
+	LOG_INF("Fetch + Read");
+
+	/* A few polling readings */
+	for (int i = 0; i < 10; i++) {
+
+		/* Fetch everything */
+		sensor_sample_fetch_chan(icm42688, SENSOR_CHAN_ALL);
+
+		sensor_channel_get(icm42688, SENSOR_CHAN_ACCEL_XYZ, accel);
+		sensor_channel_get(icm42688, SENSOR_CHAN_GYRO_XYZ, gyro);
+		sensor_channel_get(icm42688, SENSOR_CHAN_DIE_TEMP, &temp);
+
+		LOG_INF("ICM42688: Accel (m/s^2): x: %d.%06d, y: %d.%06d, z: %d.%06d",
+			accel[0].val1, accel[0].val2,
+			accel[1].val1, accel[1].val2,
+			accel[2].val1, accel[2].val2);
+		LOG_INF("ICM42688: Gyro (rad/s): x: %d.%06d, y: %d.%06d, z: %d.%06di",
+			gyro[0].val1, gyro[0].val2,
+			gyro[1].val1, gyro[1].val2,
+			gyro[2].val1, gyro[2].val2);
+		LOG_INF("ICM42688: Temp (C): %d.%06d\n",
+			temp.val1, temp.val2);
+
+		k_sleep(K_MSEC(100));
+	}
+
+	LOG_INF("Showing under/over flows with 1 buffer queue");
+	fifo_stream(icm42688, &r1, 1);
+
+	LOG_INF("Showing no under/over flows with 4 buffer queue");
+	fifo_stream(icm42688, &r4, 4);
+
+	LOG_INF("Done!");
+
+	while (true) {
+		k_msleep(1000);
+	}
+}