From 208c3f690ccb3c14474a996ec02748d6a3c7803e Mon Sep 17 00:00:00 2001 From: Tom Burdick Date: Tue, 20 Sep 2022 10:29:26 -0500 Subject: [PATCH 1/4] sensor: RTIO API support for FIFO streaming Provides the necessary APIs to stream a sensors FIFO using RTIO Adds an API call to enable explicitly reading the fifo from the sensor at any time. The thinking is that while the sensors gpio trigger might be one source of triggering a fifo read, external sources should be possible. Signed-off-by: Tom Burdick --- include/zephyr/drivers/sensor.h | 105 ++++++++++++++++++++++++++++++++ 1 file changed, 105 insertions(+) diff --git a/include/zephyr/drivers/sensor.h b/include/zephyr/drivers/sensor.h index e7f5ceba5bfe9..f6101797ef2fc 100644 --- a/include/zephyr/drivers/sensor.h +++ b/include/zephyr/drivers/sensor.h @@ -21,6 +21,7 @@ #include #include +#include #include #ifdef __cplusplus @@ -318,6 +319,10 @@ enum sensor_attribute { * to the new sampling frequency. */ SENSOR_ATTR_FF_DUR, + /** FIFO watermark attribute */ + SENSOR_ATTR_FIFO_WATERMARK, + /** FIFO enable/disable fifo related interrupts */ + SENSOR_ATTR_FIFO_INTERRUPT, /** * Number of all common sensor attributes. */ @@ -394,12 +399,48 @@ typedef int (*sensor_channel_get_t)(const struct device *dev, enum sensor_channel chan, struct sensor_value *val); +#ifdef CONFIG_RTIO + +/** + * @type sensor_fifo_iodev_t + * @brief Callback API for obtaining a reference to a sensors FIFO iodev. + */ +typedef int (*sensor_fifo_iodev_t)(const struct device *dev, struct rtio_iodev **iodev_sqe); + +/** + * @type sensor_fifo_read_t + * @brief Callback API for immediately triggering a FIFO read. + */ +typedef int (*sensor_fifo_read_t)(const struct device *dev); + +/** + * @type sensor_fifo_start_t + * @brief Callback API to start FIFO streaming. + */ +typedef int (*sensor_fifo_start_t)(const struct device *dev); + +/** + * @type sensor_fifo_stop_t + * @brief Callback API to stop FIFO streaming. + */ +typedef int (*sensor_fifo_stop_t)(const struct device *dev); + + +#endif /* CONFIG_RTIO */ + __subsystem struct sensor_driver_api { sensor_attr_set_t attr_set; sensor_attr_get_t attr_get; sensor_trigger_set_t trigger_set; sensor_sample_fetch_t sample_fetch; sensor_channel_get_t channel_get; + +#ifdef CONFIG_RTIO + sensor_fifo_iodev_t fifo_iodev; + sensor_fifo_read_t fifo_read; + sensor_fifo_start_t fifo_start; + sensor_fifo_stop_t fifo_stop; +#endif /* CONFIG_RTIO */ }; /** @@ -598,6 +639,70 @@ static inline int z_impl_sensor_channel_get(const struct device *dev, return api->channel_get(dev, chan, val); } +/** + * @brief Submit a request for the fifo stream (iodev) for this sensor. + */ +static inline int sensor_fifo_iodev(const struct device *dev, struct rtio_iodev **iodev) +{ +#ifdef CONFIG_RTIO + + const struct sensor_driver_api *api = + (const struct sensor_driver_api *)dev->api; + + return api->fifo_iodev(dev, iodev); +#else + return -ENOSYS; +#endif +} + +/** + * @brief Checkin a rtio_iodev pointer for this sensor, only one per device. + */ +static inline int sensor_fifo_read(const struct device *dev) +{ +#ifdef CONFIG_RTIO + + const struct sensor_driver_api *api = + (const struct sensor_driver_api *)dev->api; + + return api->fifo_read(dev); +#else + return -ENOSYS; +#endif +} + +/** + * @brief Start streaming the fifo for this sensor. + */ +static inline int sensor_fifo_start(const struct device *dev) +{ +#ifdef CONFIG_RTIO + + const struct sensor_driver_api *api = + (const struct sensor_driver_api *)dev->api; + + return api->fifo_start(dev); +#else + return -ENOSYS; +#endif +} + +/** + * @brief Stop streaming the fifo for this sensor. + */ +static inline int sensor_fifo_stop(const struct device *dev) +{ +#ifdef CONFIG_RTIO + + const struct sensor_driver_api *api = + (const struct sensor_driver_api *)dev->api; + + return api->fifo_stop(dev); +#else + return -ENOSYS; +#endif +} + /** * @brief The value of gravitational constant in micro m/s^2. */ From 4661440088777ec86bd263e9f08897b158798c86 Mon Sep 17 00:00:00 2001 From: Tom Burdick Date: Fri, 3 Mar 2023 16:17:29 -0600 Subject: [PATCH 2/4] icm42688: RTIO device for streaming An experimental idea to enable streaming fifo data from the sensor using RTIO with the existing sensor API. A sensor reader could easily be built on top of the buffer to produce meaningful values. There are many ways this can be built currently. Using either the existing trigger work context (system workqueue or own thread) when SPI_RTIO=n or using a sequence. With or without SPI_SAM_DMA (where only polling SPI transfers are used). This is a really nice setup to explore and experiment with RTIO, SPI, and a really fast sensor. Signed-off-by: Tom Burdick --- drivers/sensor/icm42688/Kconfig | 8 + drivers/sensor/icm42688/icm42688.c | 535 +++++++++++++++++++-- drivers/sensor/icm42688/icm42688_trigger.c | 23 +- 3 files changed, 532 insertions(+), 34 deletions(-) diff --git a/drivers/sensor/icm42688/Kconfig b/drivers/sensor/icm42688/Kconfig index a4ee90efb70c8..bb336e440e9d9 100644 --- a/drivers/sensor/icm42688/Kconfig +++ b/drivers/sensor/icm42688/Kconfig @@ -4,6 +4,8 @@ # # SPDX-License-Identifier: Apache-2.0 +DT_COMPAT_ICM42688 := invensense,icm42688 + config ICM42688 bool "ICM42688 Six-Axis Motion Tracking Device" default y @@ -58,4 +60,10 @@ config ICM42688_THREAD_STACK_SIZE help The thread stack size. +config ICM42688_RTIO + bool "ICM42688 RTIO Support" + depends on RTIO + help + Enable RTIO support for ICM42688 allowing for scatter gather like requests against the fifo. + endif # ICM42688 diff --git a/drivers/sensor/icm42688/icm42688.c b/drivers/sensor/icm42688/icm42688.c index 329907a010d93..0cebfe72ebbf8 100644 --- a/drivers/sensor/icm42688/icm42688.c +++ b/drivers/sensor/icm42688/icm42688.c @@ -8,10 +8,14 @@ #define DT_DRV_COMPAT invensense_icm42688 +#include #include +#include #include #include - +#include +#include +#include #include "icm42688.h" #include "icm42688_reg.h" #include "icm42688_spi.h" @@ -24,8 +28,37 @@ struct icm42688_sensor_data { struct icm42688_dev_data dev_data; int16_t readings[7]; + +#ifdef CONFIG_ICM42688_RTIO + const struct device *dev; + struct rtio *r; + struct rtio_iodev *fifo_iodev; + struct rtio_iodev_sqe *iodev_sqe; + bool checked_out; + uint32_t overflows; + struct gpio_callback gpio_cb; +#ifdef CONFIG_SPI_RTIO + struct rtio_iodev *spi_iodev; + uint8_t int_status; + uint16_t fifo_count; + atomic_t reading_fifo; +#endif /* CONFIG_SPI_RTIO */ +#endif /* CONFIG_ICM42688_RTIO */ + +#if CONFIG_ICM42688_TRIGGER_OWN_THREAD + struct k_thread thread; + + K_KERNEL_STACK_MEMBER(thread_stack, CONFIG_ICM42688_THREAD_STACK_SIZE); +#endif }; + +#if defined(CONFIG_SPI_RTIO) && defined(CONFIG_ICM42688_RTIO) +#define ICM42688_USE_SPI_RTIO 1 +#else +#define ICM42688_USE_SPI_RTIO 0 +#endif + struct icm42688_sensor_config { struct icm42688_dev_cfg dev_cfg; }; @@ -93,21 +126,9 @@ static int icm42688_channel_get(const struct device *dev, enum sensor_channel ch static int icm42688_sample_fetch(const struct device *dev, enum sensor_channel chan) { - uint8_t status; struct icm42688_sensor_data *data = dev->data; - const struct icm42688_sensor_config *cfg = dev->config; - - int res = icm42688_spi_read(&cfg->dev_cfg.spi, REG_INT_STATUS, &status, 1); - - if (res) { - return res; - } - - if (!FIELD_GET(BIT_INT_STATUS_DATA_RDY, status)) { - return -EBUSY; - } - uint8_t readings[14]; + int res; res = icm42688_read_all(dev, readings); @@ -215,6 +236,430 @@ static int icm42688_attr_get(const struct device *dev, enum sensor_channel chan, return res; } +#ifdef CONFIG_ICM42688_RTIO + + +static int icm42688_fifo_iodev(const struct device *dev, struct rtio_iodev **iodev) +{ + struct icm42688_sensor_data *data = dev->data; + + *iodev = data->fifo_iodev; + + return 0; +} + +static int icm42688_fifo_read(const struct device *dev) +{ + /** TODO initiate a fifo read without checking int status */ + + return 0; +} + + +static int icm42688_fifo_start(const struct device *dev) +{ + struct icm42688_dev_data *data = dev->data; + const struct icm42688_dev_cfg *cfg = dev->config; + int res; + + /* Disable gpio interrupt */ + res = gpio_pin_interrupt_configure_dt(&cfg->gpio_int1, GPIO_INT_DISABLE); + if (res != 0) { + goto out; + } + + /* Enable FIFO on chip */ + struct icm42688_cfg sensor_cfg = data->cfg; + + sensor_cfg.fifo_en = true; + + /* TODO figure out a nice way of configuring these with attr set e.g. + * fifo_odr, fifo_wm (in bytes or records?) attrs? + */ + sensor_cfg.fifo_wm = 1024; /* wm in bytes */ + sensor_cfg.accel_odr = ICM42688_ACCEL_ODR_32000; + sensor_cfg.gyro_odr = ICM42688_GYRO_ODR_32000; + + res = icm42688_safely_configure(dev, &sensor_cfg); + if (res != 0) { + goto out; + } + + /* Enable the gpio interrupt */ + res = gpio_pin_interrupt_configure_dt(&cfg->gpio_int1, GPIO_INT_EDGE_TO_ACTIVE); + +out: + return res; +} + +static int icm42688_fifo_stop(const struct device *dev) +{ + struct icm42688_sensor_data *data = dev->data; + const struct icm42688_sensor_config *cfg = dev->config; + int res; + + /* Disable gpio interrupt */ + res = gpio_pin_interrupt_configure_dt(&cfg->dev_cfg.gpio_int1, GPIO_INT_DISABLE); + + + /* Disable FIFO on chip */ + struct icm42688_cfg sensor_cfg = data->dev_data.cfg; + + sensor_cfg.fifo_en = false; + /* TODO figure out a nice way of configuring these with saved attrs e.g. xyz odrs attrs? */ + sensor_cfg.accel_odr = ICM42688_ACCEL_ODR_1000; + sensor_cfg.gyro_odr = ICM42688_GYRO_ODR_1000; + + res = icm42688_safely_configure(dev, &sensor_cfg); + if (res != 0) { + goto out; + } + + /* Clear request queue */ + rtio_iodev_cancel_all(data->fifo_iodev); + + /* TODO Possibly re-enable gpio interrupt if trigger_set has been setup */ + +out: + return 0; +} +/* Define a simple minimum buffer size of 1 fifo packet plus a copy of the config */ +#define ICM42688_MAX_FIFO_PKT_SIZE 20 +#define ICM42688_MIN_BUF_SIZE (ICM42688_MAX_FIFO_PKT_SIZE + sizeof(struct icm42688_cfg)) + +/* Accepts read requests with buffers long enough to store at least a single FIFO packet + * and appends them to a pending request queue. This is then pulled from and read into + * when sensor_fifo_read is called. + */ +void icm42688_fifo_submit(struct rtio_iodev_sqe *iodev_sqe) +{ + const struct rtio_sqe *sqe = iodev_sqe->sqe; + + if (sqe->op != RTIO_OP_RX || sqe->buf_len < ICM42688_MIN_BUF_SIZE) { + rtio_iodev_sqe_err(iodev_sqe, -EINVAL); + return; + } + + rtio_mpsc_push((struct rtio_mpsc *)&iodev_sqe->sqe->iodev->iodev_sq, &iodev_sqe->q); +} + + +static const struct rtio_iodev_api icm42688_fifo_iodev_api = { + .submit = icm42688_fifo_submit +}; + +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; +} __attribute__((__packed__)); + +BUILD_ASSERT(sizeof(struct fifo_header) == 3); + + +#if CONFIG_SPI_RTIO + +static void icm42688_complete_cb(struct rtio *r, const struct rtio_sqe *sqe, void *arg) +{ + const struct device *dev = arg; + struct icm42688_sensor_data *drv_data = dev->data; + const struct icm42688_sensor_config *drv_cfg = dev->config; + + /* TODO report number of frames or bytes read here */ + rtio_iodev_sqe_ok(drv_data->iodev_sqe, drv_data->fifo_count); + drv_data->iodev_sqe = NULL; + + gpio_pin_interrupt_configure_dt(&drv_cfg->dev_cfg.gpio_int1, GPIO_INT_EDGE_TO_ACTIVE); +} + +static void icm42688_fifo_count_cb(struct rtio *r, const struct rtio_sqe *sqe, void *arg) +{ + const struct device *dev = arg; + struct icm42688_sensor_data *drv_data = dev->data; + const struct icm42688_sensor_config *drv_cfg = dev->config; + uint8_t *fifo_count_buf = (uint8_t *)&drv_data->fifo_count; + uint16_t fifo_count = ((fifo_count_buf[0] << 8) | fifo_count_buf[1]); + + drv_data->fifo_count = fifo_count; + + /* Pull a operation from our device iodev queue, validated to only be reads */ + struct rtio_mpsc_node *next = rtio_mpsc_pop(&drv_data->fifo_iodev->iodev_sq); + + /* Not inherently an underrun/overrun as we may have a buffer to fill next time */ + if (next == NULL) { + gpio_pin_interrupt_configure_dt(&drv_cfg->dev_cfg.gpio_int1, + GPIO_INT_EDGE_TO_ACTIVE); + return; + } + + struct rtio_iodev_sqe *iodev_sqe = CONTAINER_OF(next, struct rtio_iodev_sqe, q); + + drv_data->iodev_sqe = iodev_sqe; + + const size_t packet_size = drv_data->dev_data.cfg.fifo_hires ? 20 : 16; + + /* Check buffer is big enough for configuration + * and at least 1 packet + * + * TODO determine len to be aligned down to the nearest packet + */ + if (iodev_sqe->sqe->buf_len < (sizeof(struct fifo_header) + packet_size)) { + LOG_WRN("Buffer minimum size not met"); + rtio_iodev_sqe_err(iodev_sqe, -ENOMEM); + return; + } + + /* Read FIFO and call back to rtio with rtio_sqe completion */ + /* TODO is packet format even needed? the fifo has a header per packet + * already + */ + struct fifo_header hdr = { + .int_status = drv_data->int_status, + .gyro_odr = drv_data->dev_data.cfg.gyro_odr, + .gyro_fs = drv_data->dev_data.cfg.gyro_fs, + .accel_odr = drv_data->dev_data.cfg.accel_odr, + .accel_fs = drv_data->dev_data.cfg.accel_fs, + .packet_format = 0, + }; + + uint32_t buf_avail = iodev_sqe->sqe->buf_len; + + memcpy(iodev_sqe->sqe->buf, &hdr, sizeof(hdr)); + buf_avail -= sizeof(hdr); + + /* TODO account for other packet sizes */ + const uint32_t pkt_size = 16; + + uint32_t read_len = fifo_count > buf_avail ? buf_avail : fifo_count; + uint32_t pkts = read_len / pkt_size; + + read_len = pkts*pkt_size; + + __ASSERT_NO_MSG(read_len % pkt_size == 0); + + uint8_t *read_buf = &iodev_sqe->sqe->buf[sizeof(hdr)]; + + /* Flush out completions */ + struct rtio_cqe *cqe = rtio_cqe_consume(drv_data->r); + + while (cqe != NULL) { + cqe = rtio_cqe_consume(drv_data->r); + } + rtio_cqe_release_all(r); + + /* Setup new rtio chain to read the fifo data and report then check the + * result + */ + struct rtio_sqe *write_fifo_addr = rtio_sqe_acquire(drv_data->r); + struct rtio_sqe *read_fifo_data = rtio_sqe_acquire(drv_data->r); + struct rtio_sqe *complete_op = rtio_sqe_acquire(drv_data->r); + const uint8_t reg_addr = REG_SPI_READ_BIT | FIELD_GET(REG_ADDRESS_MASK, REG_FIFO_DATA); + + rtio_sqe_prep_tiny_write(write_fifo_addr, drv_data->spi_iodev, + RTIO_PRIO_NORM, ®_addr, 1, NULL); + write_fifo_addr->flags = RTIO_SQE_TRANSACTION; + rtio_sqe_prep_read(read_fifo_data, drv_data->spi_iodev, RTIO_PRIO_NORM, + read_buf, read_len, iodev_sqe); + /* TODO add flush op here, works without because we are using simple executor */ + rtio_sqe_prep_callback(complete_op, icm42688_complete_cb, + (void *)dev, NULL); + + rtio_submit(drv_data->r, 0); +} + +static void icm42688_int_status_cb(struct rtio *r, const struct rtio_sqe *sqe, void *arg) +{ + const struct device *dev = arg; + struct icm42688_sensor_data *drv_data = dev->data; + const struct icm42688_sensor_config *drv_cfg = dev->config; + + if (!(drv_data->int_status & BIT_INT_STATUS_FIFO_THS + || drv_data->int_status & BIT_INT_STATUS_FIFO_FULL)) { + gpio_pin_interrupt_configure_dt(&drv_cfg->dev_cfg.gpio_int1, + GPIO_INT_EDGE_TO_ACTIVE); + return; + } + + /* In effect a overrun, as the sensor is producing faster than we are consuming */ + if (drv_data->int_status & BIT_INT_STATUS_FIFO_FULL) { + drv_data->overflows++; + } + + /* Flush out completions */ + struct rtio_cqe *cqe = rtio_cqe_consume(drv_data->r); + + while (cqe != NULL) { + cqe = rtio_cqe_consume(drv_data->r); + } + rtio_cqe_release_all(r); + + struct rtio_sqe *write_fifo_count_reg = rtio_sqe_acquire(drv_data->r); + struct rtio_sqe *read_fifo_count = rtio_sqe_acquire(drv_data->r); + struct rtio_sqe *check_fifo_count = rtio_sqe_acquire(drv_data->r); + uint8_t reg = REG_SPI_READ_BIT | FIELD_GET(REG_ADDRESS_MASK, REG_FIFO_COUNTH); + uint8_t *read_buf = (uint8_t *)&drv_data->fifo_count; + + rtio_sqe_prep_tiny_write(write_fifo_count_reg, drv_data->spi_iodev, RTIO_PRIO_NORM, + ®, 1, NULL); + write_fifo_count_reg->flags = RTIO_SQE_TRANSACTION; + rtio_sqe_prep_read(read_fifo_count, drv_data->spi_iodev, RTIO_PRIO_NORM, + read_buf, 2, NULL); + /* TODO add flush op here, works without because we are using simple executor */ + rtio_sqe_prep_callback(check_fifo_count, icm42688_fifo_count_cb, arg, NULL); + + rtio_submit(drv_data->r, 0); +} + +void icm42688_rtio_fifo_event(const struct device *dev) +{ + struct icm42688_sensor_data *drv_data = dev->data; + + /* + * Setup rtio chain of ops with inline calls to make decisions + * 1. read int status + * 2. call to check int status and get pending RX operation + * 4. read fifo len + * 5. call to determine read len + * 6. read fifo + * 7. call to report completion + */ + struct rtio_sqe *write_int_reg = rtio_sqe_acquire(drv_data->r); + struct rtio_sqe *read_int_reg = rtio_sqe_acquire(drv_data->r); + struct rtio_sqe *check_int_status = rtio_sqe_acquire(drv_data->r); + uint8_t reg = REG_SPI_READ_BIT | FIELD_GET(REG_ADDRESS_MASK, REG_INT_STATUS); + + rtio_sqe_prep_tiny_write(write_int_reg, drv_data->spi_iodev, RTIO_PRIO_NORM, ®, 1, NULL); + write_int_reg->flags = RTIO_SQE_TRANSACTION; + rtio_sqe_prep_read(read_int_reg, drv_data->spi_iodev, RTIO_PRIO_NORM, + &drv_data->int_status, 1, NULL); + /* TODO add flush op here, works without because we are using simple executor */ + rtio_sqe_prep_callback(check_int_status, icm42688_int_status_cb, (void *)dev, NULL); + + rtio_submit(drv_data->r, 0); +} + +#else + +void icm42688_rtio_fifo_event(const struct device *dev) +{ + struct icm42688_sensor_data *data = dev->data; + const struct icm42688_sensor_config *cfg = dev->config; + + int res = 0; + uint16_t fifo_count, fifo_lost; + uint8_t int_status; + uint8_t read_buf[2]; + + res = icm42688_spi_read(&cfg->dev_cfg.spi, REG_INT_STATUS, &int_status, 1); + if (res) { + goto out; + } + + if (data->dev_data.cfg.fifo_en != true) { + goto out; + } + + if (!(int_status & BIT_INT_STATUS_FIFO_THS || int_status & BIT_INT_STATUS_FIFO_FULL)) { + goto out; + } + + /* In effect a overrun, as the sensor is producing faster than we are consuming */ + if (int_status & BIT_INT_STATUS_FIFO_FULL) { + data->overflows++; + } + + res = icm42688_spi_read(&cfg->dev_cfg.spi, REG_FIFO_COUNTH, read_buf, 2); + + if (res) { + goto out; + } + fifo_count = ((read_buf[0] << 8) | read_buf[1]); + + if (fifo_count < data->dev_data.cfg.fifo_wm) { + goto out; + } + + /* Get a buffer to read into, if one exists */ + struct rtio_mpsc_node *next = rtio_mpsc_pop(&data->fifo_iodev->iodev_sq); + + /* Not inherently an underrun/overrun as we may have a buffer to fill + * next time + */ + if (next == NULL) { + /* In effect we want to punish this thread (ugh) for now so + * others may run. Without this its very possible all time is + * spent jumping from the interrupt to this thread not allowing + * other threads to run (like the one adding the buffers to + * read into) + */ + k_msleep(1); + goto out; + } + + struct rtio_iodev_sqe *iodev_sqe = CONTAINER_OF(next, struct rtio_iodev_sqe, q); + + /* Check buffer is big enough for configuration + * and at least 1 packet + * + * TODO determine len to be aligned down to the nearest packet + */ + if (iodev_sqe->sqe->buf_len < (sizeof(struct fifo_header) + 20)) { + LOG_WRN("Buffer minimum size not met"); + rtio_iodev_sqe_err(iodev_sqe, -ENOMEM); + goto out; + } + + /* Read FIFO and call back to rtio with rtio_sqe completion */ + /* TODO is packet format even needed? the fifo has a header per packet + * already + */ + struct fifo_header hdr = { + .int_status = int_status, + .gyro_odr = data->dev_data.cfg.gyro_odr, + .gyro_fs = data->dev_data.cfg.gyro_fs, + .accel_odr = data->dev_data.cfg.accel_odr, + .accel_fs = data->dev_data.cfg.accel_fs, + .packet_format = 0, + }; + + uint32_t buf_avail = iodev_sqe->sqe->buf_len; + + memcpy(iodev_sqe->sqe->buf, &hdr, sizeof(hdr)); + buf_avail -= sizeof(hdr); + + /* TODO account for other packet sizes */ + const uint32_t pkt_size = 16; + uint32_t read_len = fifo_count > buf_avail ? buf_avail : fifo_count; + uint32_t pkts = read_len / pkt_size; + + read_len = pkts*pkt_size; + + __ASSERT_NO_MSG(read_len % pkt_size == 0); + + uint8_t *read_addr = &iodev_sqe->sqe->buf[sizeof(hdr)]; + + res = icm42688_spi_read(&cfg->dev_cfg.spi, REG_FIFO_DATA, read_addr, + read_len); + + if (res != 0) { + LOG_ERR("Error reading fifo"); + rtio_iodev_sqe_err(iodev_sqe, -EIO); + goto out; + } + + rtio_iodev_sqe_ok(iodev_sqe, read_len + sizeof(hdr)); + +out: + return; +} + +#endif /* CONFIG_SPI_RTIO */ +#endif /* CONFIG_ICM42688_RTIO */ + + static const struct sensor_driver_api icm42688_driver_api = { .sample_fetch = icm42688_sample_fetch, .channel_get = icm42688_channel_get, @@ -222,7 +667,13 @@ static const struct sensor_driver_api icm42688_driver_api = { .attr_get = icm42688_attr_get, #ifdef CONFIG_ICM42688_TRIGGER .trigger_set = icm42688_trigger_set, -#endif +#endif /* CONFIG_ICM42688_TRIGGER */ +#ifdef CONFIG_ICM42688_RTIO + .fifo_iodev = icm42688_fifo_iodev, + .fifo_read = icm42688_fifo_read, + .fifo_start = icm42688_fifo_start, + .fifo_stop = icm42688_fifo_stop, +#endif /* CONFIG_ICM42688_RTIO */ }; int icm42688_init(const struct device *dev) @@ -268,7 +719,7 @@ int icm42688_init(const struct device *dev) return res; } - return 0; + return res; } #ifndef CONFIG_ICM42688_TRIGGER @@ -280,26 +731,48 @@ void icm42688_unlock(const struct device *dev) { ARG_UNUSED(dev); } -#endif +#endif /* CONFIG_ICM42688_TRIGGER */ /* device defaults to spi mode 0/3 support */ -#define ICM42688_SPI_CFG \ +#define ICM42688_SPI_CFG \ SPI_OP_MODE_MASTER | SPI_MODE_CPOL | SPI_MODE_CPHA | SPI_WORD_SET(8) | SPI_TRANSFER_MSB +#define ICM42688_RTIO_DEFINE(inst) \ + IF_ENABLED(CONFIG_SPI_RTIO, \ + (SPI_DT_IODEV_DEFINE(icm42688_spi_iodev_##inst, \ + DT_DRV_INST(inst), ICM42688_SPI_CFG, 0U));) \ + RTIO_EXECUTOR_SIMPLE_DEFINE(icm42688_rtio_exec_##inst); \ + RTIO_DEFINE(icm42688_rtio_##inst, \ + (struct rtio_executor *)&icm42688_rtio_exec_##inst, 8, 4); \ + RTIO_IODEV_DEFINE(icm42688_fifo_iodev_##inst, &icm42688_fifo_iodev_api, NULL); + +#define ICM42688_DEFINE_CONFIG(inst) \ + static const struct icm42688_sensor_config icm42688_cfg_##inst = { \ + .dev_cfg = \ + { \ + .spi = SPI_DT_SPEC_INST_GET(inst, ICM42688_SPI_CFG, 0U), \ + .gpio_int1 = GPIO_DT_SPEC_INST_GET_OR(inst, int_gpios, {0}), \ + }, \ + } + +#define ICM42688_DEFINE_DATA(inst) \ + IF_ENABLED(CONFIG_ICM42688_RTIO, (ICM42688_RTIO_DEFINE(inst))); \ + static struct icm42688_sensor_data icm42688_driver_##inst = { \ + IF_ENABLED(CONFIG_ICM42688_RTIO, ( \ + .r = &icm42688_rtio_##inst, \ + .fifo_iodev = &icm42688_fifo_iodev_##inst, \ + )) \ + IF_ENABLED(ICM42688_USE_SPI_RTIO, ( \ + .spi_iodev = &icm42688_spi_iodev_##inst, \ + )) \ + } -#define ICM42688_INIT(inst) \ - static struct icm42688_sensor_data icm42688_driver_##inst = { 0 }; \ - \ - static const struct icm42688_sensor_config icm42688_cfg_##inst = { \ - .dev_cfg = \ - { \ - .spi = SPI_DT_SPEC_INST_GET(inst, ICM42688_SPI_CFG, 0U), \ - .gpio_int1 = GPIO_DT_SPEC_INST_GET_OR(inst, int_gpios, {0}), \ - }, \ - }; \ - \ - SENSOR_DEVICE_DT_INST_DEFINE(inst, icm42688_init, NULL, &icm42688_driver_##inst, \ - &icm42688_cfg_##inst, POST_KERNEL, \ +#define ICM42688_INIT(inst) \ + ICM42688_DEFINE_CONFIG(inst); \ + ICM42688_DEFINE_DATA(inst); \ + \ + SENSOR_DEVICE_DT_INST_DEFINE(inst, icm42688_init, NULL, &icm42688_driver_##inst, \ + &icm42688_cfg_##inst, POST_KERNEL, \ CONFIG_SENSOR_INIT_PRIORITY, &icm42688_driver_api); DT_INST_FOREACH_STATUS_OKAY(ICM42688_INIT) diff --git a/drivers/sensor/icm42688/icm42688_trigger.c b/drivers/sensor/icm42688/icm42688_trigger.c index 01e555a3f82f0..8c20deb323ec0 100644 --- a/drivers/sensor/icm42688/icm42688_trigger.c +++ b/drivers/sensor/icm42688/icm42688_trigger.c @@ -17,18 +17,32 @@ LOG_MODULE_DECLARE(ICM42688, CONFIG_SENSOR_LOG_LEVEL); + +void icm42688_rtio_fifo_event(const struct device *dev); + static void icm42688_gpio_callback(const struct device *dev, struct gpio_callback *cb, uint32_t pins) { struct icm42688_dev_data *data = CONTAINER_OF(cb, struct icm42688_dev_data, gpio_cb); + const struct icm42688_dev_cfg *cfg = data->dev->config; ARG_UNUSED(dev); ARG_UNUSED(pins); + gpio_pin_interrupt_configure_dt(&cfg->gpio_int1, GPIO_INT_DISABLE); + +#if defined(CONFIG_SPI_RTIO) && defined(CONFIG_ICM42688_RTIO) + if (data->cfg.fifo_en) { + icm42688_rtio_fifo_event(data->dev); + } else { +#endif #if defined(CONFIG_ICM42688_TRIGGER_OWN_THREAD) - k_sem_give(&data->gpio_sem); + k_sem_give(&data->gpio_sem); #elif defined(CONFIG_ICM42688_TRIGGER_GLOBAL_THREAD) - k_work_submit(&data->work); + k_work_submit(&data->work); +#endif +#if defined(CONFIG_SPI_RTIO) && defined(CONFIG_ICM42688_RTIO) + } #endif } @@ -39,7 +53,9 @@ static void icm42688_thread_cb(const struct device *dev) icm42688_lock(dev); - if (data->data_ready_handler != NULL) { + if (data->cfg.fifo_en && IS_ENABLED(CONFIG_ICM42688_RTIO) && !IS_ENABLED(CONFIG_SPI_RTIO)) { + icm42688_rtio_fifo_event(dev); + } else if (data->data_ready_handler != NULL) { data->data_ready_handler(dev, data->data_ready_trigger); } else { uint8_t status; @@ -47,6 +63,7 @@ static void icm42688_thread_cb(const struct device *dev) icm42688_spi_read(&cfg->spi, REG_INT_STATUS, &status, 1); } + gpio_pin_interrupt_configure_dt(&cfg->gpio_int1, GPIO_INT_EDGE_TO_ACTIVE); icm42688_unlock(dev); } From c291c3556bed65866f1849a64c7a195d64a7feda Mon Sep 17 00:00:00 2001 From: Tom Burdick Date: Fri, 3 Mar 2023 17:45:21 -0600 Subject: [PATCH 3/4] 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 --- samples/boards/tdk_robokit1/CMakeLists.txt | 11 ++ samples/boards/tdk_robokit1/README.rst | 22 +++ samples/boards/tdk_robokit1/prj.conf | 37 ++++ samples/boards/tdk_robokit1/sample.yaml | 8 + samples/boards/tdk_robokit1/src/main.c | 190 +++++++++++++++++++++ 5 files changed, 268 insertions(+) create mode 100644 samples/boards/tdk_robokit1/CMakeLists.txt create mode 100644 samples/boards/tdk_robokit1/README.rst create mode 100644 samples/boards/tdk_robokit1/prj.conf create mode 100644 samples/boards/tdk_robokit1/sample.yaml create mode 100644 samples/boards/tdk_robokit1/src/main.c diff --git a/samples/boards/tdk_robokit1/CMakeLists.txt b/samples/boards/tdk_robokit1/CMakeLists.txt new file mode 100644 index 0000000000000..a4b5a2fcf7550 --- /dev/null +++ b/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) diff --git a/samples/boards/tdk_robokit1/README.rst b/samples/boards/tdk_robokit1/README.rst new file mode 100644 index 0000000000000..1af271d0cdb9f --- /dev/null +++ b/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. diff --git a/samples/boards/tdk_robokit1/prj.conf b/samples/boards/tdk_robokit1/prj.conf new file mode 100644 index 0000000000000..cdd06aaab9327 --- /dev/null +++ b/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 diff --git a/samples/boards/tdk_robokit1/sample.yaml b/samples/boards/tdk_robokit1/sample.yaml new file mode 100644 index 0000000000000..f0886b70705d5 --- /dev/null +++ b/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 diff --git a/samples/boards/tdk_robokit1/src/main.c b/samples/boards/tdk_robokit1/src/main.c new file mode 100644 index 0000000000000..7e22540fa7be3 --- /dev/null +++ b/samples/boards/tdk_robokit1/src/main.c @@ -0,0 +1,190 @@ +/* + * Copyright (c) 2022 Intel Corporation + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include +#include +#include +#include + +#include +#include + +#include +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); + } +} From a82d29756944a2dc566e9125b760de335be831dd Mon Sep 17 00:00:00 2001 From: Tom Burdick Date: Fri, 10 Mar 2023 08:17:18 -0600 Subject: [PATCH 4/4] samples: tdk_robokit1: Poll the magnetometer While an asynchronous transfer is on going for the 6 axis imu, concurrently read the magnetometer with its blocking fetch call. In the future it'd be nice to have the magnetometer also work with RTIO driven by a timer. In fact having all the sensors read on a timer would be incredibly nice to showcase. Signed-off-by: Tom Burdick --- samples/boards/tdk_robokit1/prj.conf | 19 +++-- samples/boards/tdk_robokit1/src/main.c | 106 +++++++++++++++++-------- 2 files changed, 86 insertions(+), 39 deletions(-) diff --git a/samples/boards/tdk_robokit1/prj.conf b/samples/boards/tdk_robokit1/prj.conf index cdd06aaab9327..49a9bcdae9333 100644 --- a/samples/boards/tdk_robokit1/prj.conf +++ b/samples/boards/tdk_robokit1/prj.conf @@ -11,10 +11,12 @@ CONFIG_LOG_MODE_DEFERRED=y CONFIG_SPI=y CONFIG_I2C=y CONFIG_GPIO=y +CONFIG_THREAD_NAME=y +CONFIG_DEBUG_THREAD_INFO=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 @@ -25,13 +27,20 @@ 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_SUBMIT_SEM=n CONFIG_RTIO_CONSUME_SEM=y -# SystemView Tracing +# Tracing CONFIG_TRACING=y CONFIG_TRACING_TIMER=y CONFIG_TRACING_SYSCALL=n -CONFIG_SEGGER_SYSTEMVIEW=y -CONFIG_SEGGER_SYSVIEW_RTT_BUFFER_SIZE=65536 + +# SystemView +#CONFIG_SEGGER_SYSTEMVIEW=y +#CONFIG_SEGGER_SYSVIEW_RTT_BUFFER_SIZE=65536 + +# Threadalyzer Tracing + +# Percepio +CONFIG_PERCEPIO_TRACERECORDER=y diff --git a/samples/boards/tdk_robokit1/src/main.c b/samples/boards/tdk_robokit1/src/main.c index 7e22540fa7be3..ecdb794e38c64 100644 --- a/samples/boards/tdk_robokit1/src/main.c +++ b/samples/boards/tdk_robokit1/src/main.c @@ -41,15 +41,65 @@ struct fifo_header { #define ICM42688_FIFO_BUF_LEN 2051 static uint8_t icm42688_fifo_bufs[N_BUFS][ICM42688_FIFO_BUF_LEN]; +const struct device *icm42688 = DEVICE_DT_GET_ONE(invensense_icm42688); +const struct device *akm09918 = DEVICE_DT_GET_ONE(asahi_kasei_akm09918c); -void fifo_stream(const struct device *icm42688, struct rtio *r, uint32_t n_bufs) +void poll_imu(void) +{ + uint32_t cycle_start, cycle_end; + struct sensor_value accel[3]; + struct sensor_value gyro[3]; + struct sensor_value imu_temp; + + /* Fetch everything */ + cycle_start = k_cycle_get_32(); + sensor_sample_fetch_chan(icm42688, SENSOR_CHAN_ALL); + cycle_end = k_cycle_get_32(); + + 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, &imu_temp); + + LOG_INF("ICM42688: Fetch took %u cycles", cycle_end - cycle_start); + 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.%06d", + 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", + imu_temp.val1, imu_temp.val2); +} + +void poll_mag(void) +{ + uint32_t cycle_start, cycle_end; + struct sensor_value magn[3]; + + /* Fetch everything */ + cycle_start = k_cycle_get_32(); + sensor_sample_fetch_chan(akm09918, SENSOR_CHAN_ALL); + cycle_end = k_cycle_get_32(); + + sensor_channel_get(akm09918, SENSOR_CHAN_MAGN_XYZ, magn); + + LOG_INF("AKM09918: Fetch took %u cycles", cycle_end - cycle_start); + LOG_INF("AKM09918: Mag (Gauss): x: %d.%06d, y: %d.%06d, z: %d.%06d", + magn[0].val1, magn[0].val2, + magn[1].val1, magn[1].val2, + magn[2].val1, magn[2].val2); +} + +void fifo_stream(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 */ + /* obtain reference to the stream */ (void)sensor_fifo_iodev(icm42688, &iodev); LOG_INF("Setting up RX requests"); @@ -107,11 +157,12 @@ void fifo_stream(const struct device *icm42688, struct rtio *r, uint32_t n_bufs) } - if (i % 128 == 0) { - printk("Slow mode on iteration %d, underflows (sensor overflows) %u, buf " - "%lu, data int status %x, result %d\n", + if (i % 64 == 0) { + LOG_INF("Poll Mag: Iteration %d, Underflows (sensor overflows) %u, Buf " + "%lu, int status %x, result %d\n", i, overflows, buf_idx, buf[0], result); - k_busy_wait(4000); + poll_mag(); + k_busy_wait(1000); } /* Now to recycle the buffer by putting it back in the queue */ @@ -134,53 +185,40 @@ void fifo_stream(const struct device *icm42688, struct rtio *r, uint32_t n_bufs) 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++) { + if (!device_is_ready(akm09918)) { + LOG_INF("%s: device not ready.", akm09918->name); + return; + } - /* Fetch everything */ - sensor_sample_fetch_chan(icm42688, SENSOR_CHAN_ALL); + struct sensor_value sample_freq = { .val1 = 10, .val2 = 0 }; - 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); + sensor_attr_set(akm09918, SENSOR_CHAN_MAGN_XYZ, SENSOR_ATTR_SAMPLING_FREQUENCY, + &sample_freq); - 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); + LOG_INF("Fetch + Read"); + /* A few polling readings */ + for (int i = 0; i < 10; i++) { + poll_imu(); + poll_mag(); k_sleep(K_MSEC(100)); } LOG_INF("Showing under/over flows with 1 buffer queue"); - fifo_stream(icm42688, &r1, 1); + fifo_stream(&r1, 1); LOG_INF("Showing no under/over flows with 4 buffer queue"); - fifo_stream(icm42688, &r4, 4); + fifo_stream(&r4, 4); LOG_INF("Done!");