diff --git a/drivers/sensor/CMakeLists.txt b/drivers/sensor/CMakeLists.txt index 098b57fdddef6c..17a4fe37601a3e 100644 --- a/drivers/sensor/CMakeLists.txt +++ b/drivers/sensor/CMakeLists.txt @@ -129,13 +129,8 @@ add_subdirectory_ifdef(CONFIG_XMC4XXX_TEMP xmc4xxx_temp) add_subdirectory_ifdef(CONFIG_TMD2620 tmd2620) add_subdirectory_ifdef(CONFIG_ZEPHYR_NTC_THERMISTOR zephyr_thermistor) -if(CONFIG_USERSPACE OR CONFIG_SENSOR_SHELL OR CONFIG_SENSOR_SHELL_BATTERY) -# The above if() is needed or else CMake would complain about -# empty library. - zephyr_library() +zephyr_library_sources(default_rtio_sensor.c) zephyr_library_sources_ifdef(CONFIG_USERSPACE sensor_handlers.c) zephyr_library_sources_ifdef(CONFIG_SENSOR_SHELL sensor_shell.c) zephyr_library_sources_ifdef(CONFIG_SENSOR_SHELL_BATTERY shell_battery.c) - -endif() diff --git a/drivers/sensor/default_rtio_sensor.c b/drivers/sensor/default_rtio_sensor.c new file mode 100644 index 00000000000000..0495910b86d127 --- /dev/null +++ b/drivers/sensor/default_rtio_sensor.c @@ -0,0 +1,219 @@ +/* + * Copyright (c) 2023 Google LLC. + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include + +#include +#include +#include + +LOG_MODULE_REGISTER(rtio_sensor, CONFIG_SENSOR_LOG_LEVEL); + +static void sensor_submit_fallback(const struct device *dev, struct rtio_iodev_sqe *iodev_sqe); + +static void sensor_iodev_submit(struct rtio_iodev_sqe *iodev_sqe) +{ + const struct sensor_iodev_data *data = iodev_sqe->sqe->iodev->data; + const struct device *dev = data->sensor; + const struct sensor_driver_api *api = dev->api; + + if (api->submit != NULL) { + api->submit(dev, iodev_sqe); + } else { + sensor_submit_fallback(dev, iodev_sqe); + } +} + +struct rtio_iodev_api sensor_iodev_api = { + .submit = sensor_iodev_submit, +}; + +static inline int compute_num_samples(const enum sensor_channel *channels, size_t num_channels) +{ + int num_samples = 0; + + for (size_t i = 0; i < num_channels; ++i) { + num_samples += SENSOR_CHANNEL_3_AXIS(channels[i]) ? 3 : 1; + } + + return num_samples; +} + +static void sensor_submit_fallback(const struct device *dev, struct rtio_iodev_sqe *iodev_sqe) +{ + const struct sensor_iodev_data *data = iodev_sqe->sqe->iodev->data; + const enum sensor_channel *const channels = data->channels; + const size_t num_channels = data->num_channels; + int num_output_samples = compute_num_samples(channels, num_channels); + uint32_t min_buf_len = + sizeof(struct sensor_data_generic_header) + + num_output_samples * (sizeof(q31_t) + sizeof(struct sensor_data_generic_channel)); + uint64_t timestamp_ns = k_uptime_get(); + int rc = sensor_sample_fetch(dev); + uint8_t *buf; + uint32_t buf_len; + + if (rc != 0) { + rtio_iodev_sqe_err(iodev_sqe, rc); + return; + } + if (num_output_samples != 6) { + LOG_ERR("Not 6"); + rtio_iodev_sqe_err(iodev_sqe, -1); + return; + } + LOG_DBG("Trying to allocate %u bytes for %d samples", min_buf_len, num_output_samples); + rc = rtio_sqe_rx_buf(iodev_sqe, min_buf_len, min_buf_len, &buf, &buf_len); + if (rc != 0) { + LOG_INF("Failed to allocate memory"); + rtio_iodev_sqe_err(iodev_sqe, rc); + return; + } + LOG_DBG("Buffer allocated at %p, size=%u", (void*)buf, buf_len); + + /* Set the timestamp and reset num_channels */ + struct sensor_data_generic_header *header = (struct sensor_data_generic_header *)buf; + header->timestamp_ns = timestamp_ns; + header->num_channels = num_output_samples; + LOG_DBG("Reading %d channels @%" PRIu64, (int)num_channels, timestamp_ns); + uint8_t current_channel_index = 0; + q31_t *q = (q31_t *)(buf + sizeof(struct sensor_data_generic_header) + + num_output_samples * sizeof(struct sensor_data_generic_channel)); + for (size_t i = 0; i < num_channels; ++i) { + struct sensor_value value[3]; + int num_samples = SENSOR_CHANNEL_3_AXIS(channels[i]) ? 3 : 1; + + rc = sensor_channel_get(dev, channels[i], value); + if (rc != 0) { + LOG_ERR("Failed to get channel %d", channels[i]); + rtio_iodev_sqe_err(iodev_sqe, rc); + } + + /* Package the data */ + uint32_t header_scale = 0; + for (int sample = 0; sample < num_samples; ++sample) { + uint32_t scale = (uint32_t)llabs((int64_t)value[sample].val1 + 1); + + header_scale = MAX(header_scale, scale); + } + LOG_DBG(" [%d] scale=%u", (int)i, header_scale); + for (int sample = 0; sample < num_samples; ++sample) { + header->channel_info[current_channel_index + sample].scale = header_scale; + + int64_t value_u = + value[sample].val1 * INT64_C(1000000) + value[sample].val2; + q[current_channel_index + sample] = + (value_u / header_scale) * ((INT64_C(1) << 31) - 1) / 1000000; + LOG_DBG("value[%d]=%s%d.%06d, q=%d", sample, value_u < 0 ? "-" : "", + abs((int)(value_u / 1000000)), abs((int)(value_u % 1000000)), *q); + } + current_channel_index += num_samples; + } + rtio_iodev_sqe_ok(iodev_sqe, 0); +} + +void z_sensor_processing_loop(struct rtio *ctx, sensor_processing_callback_t cb) +{ + struct device *read_dev; + uint8_t *buf = NULL; + uint32_t buf_len = 0; + int rc; + + struct rtio_cqe *cqe = rtio_cqe_consume(ctx); + + if (cqe == NULL) { + k_msleep(1); + return; + } + + rc = cqe->result; + read_dev = (struct device *)cqe->userdata; + rtio_cqe_get_mempool_buffer(ctx, cqe, &buf, &buf_len); + rtio_cqe_release(ctx); + + cb(read_dev, rc, buf, buf_len); + + rtio_release_buffer(ctx, buf); +} + +static int get_frame_count(const uint8_t *buffer, uint16_t *frame_count) +{ + *frame_count = 1; + return 0; +} + +static int get_timestamp(const uint8_t *buffer, uint64_t *timestamp_ns) +{ + *timestamp_ns = ((struct sensor_data_generic_header *)buffer)->timestamp_ns; + return 0; +} + +static inline bool is_subchannel(enum sensor_channel needle, enum sensor_channel haystack) +{ + if (needle == haystack) { + return true; + } + if (!SENSOR_CHANNEL_3_AXIS(haystack)) { + return false; + } + return (haystack == SENSOR_CHAN_ACCEL_XYZ && + (needle == SENSOR_CHAN_ACCEL_X || needle == SENSOR_CHAN_ACCEL_Y || + needle == SENSOR_CHAN_ACCEL_Z)) || + (haystack == SENSOR_CHAN_GYRO_XYZ && + (needle == SENSOR_CHAN_GYRO_X || needle == SENSOR_CHAN_GYRO_Y || + needle == SENSOR_CHAN_GYRO_Z)) || + (haystack == SENSOR_CHAN_MAGN_XYZ && + (needle == SENSOR_CHAN_MAGN_X || needle == SENSOR_CHAN_MAGN_Y || + needle == SENSOR_CHAN_MAGN_Z)); +} + +static int get_scale(const uint8_t *buffer, enum sensor_channel channel_type, uint32_t *scale) +{ + struct sensor_data_generic_header *header = (struct sensor_data_generic_header *)buffer; + + for (uint8_t i = 0; i < header->num_channels; ++i) { + if (is_subchannel(header->channel_info[i].channel, channel_type)) { + *scale = header->channel_info[i].scale; + return 0; + } + } + return -EINVAL; +} + +static int decode(const uint8_t *buffer, sensor_frame_iterator_t *fit, + sensor_channel_iterator_t *cit, enum sensor_channel *channels, q31_t *values, + uint8_t max_count) +{ + const struct sensor_data_generic_header *header = + (const struct sensor_data_generic_header *)buffer; + const q31_t *q = + (const q31_t *)(buffer + sizeof(struct sensor_data_generic_header) + + header->num_channels * sizeof(struct sensor_data_generic_channel)); + + if (*fit != 0) { + return -EINVAL; + } + + int count = 0; + for (; *cit < header->num_channels && count < max_count; ++count) { + channels[count] = header->channel_info[count].channel; + values[count] = q[count]; + *cit += 1; + } + + if (*cit >= header->num_channels) { + *fit += 1; + *cit = 0; + } + return count; +} + +struct sensor_decoder_api __sensor_default_decoder = { + .get_frame_count = get_frame_count, + .get_timestamp = get_timestamp, + .get_scale = get_scale, + .decode = decode, +}; diff --git a/include/zephyr/drivers/sensor.h b/include/zephyr/drivers/sensor.h index e7f5ceba5bfe91..04bbdd0b976b00 100644 --- a/include/zephyr/drivers/sensor.h +++ b/include/zephyr/drivers/sensor.h @@ -19,9 +19,13 @@ * @{ */ +#include +#include + #include #include -#include +#include +#include #ifdef __cplusplus extern "C" { @@ -394,12 +398,171 @@ typedef int (*sensor_channel_get_t)(const struct device *dev, enum sensor_channel chan, struct sensor_value *val); +/** + * @typedef sensor_frame_iterator_t + * @brief Used for iterating over the data frames via the :c:struct:`sensor_decoder_api` + * + * Example usage: + * + * @code(.c) + * sensor_frame_iterator_t fit = {0}, fit_last; + * sensor_channel_iterator_t cit = {0}, cit_last; + * + * while (true) { + * int num_decoded_channels; + * enum sensor_channel channel; + * q31_t value; + * + * fit_last = fit; + * num_decoded_channels = decoder->decode(buffer, &fit, &cit, &channel, &value, 1); + * + * if (num_decoded_channels <= 0) { + * printk("Done decoding buffer\n"); + * break; + * } + * + * printk("Decoded channel (%d) with value %s0.%06" PRIi64 "\n", q < 0 ? "-" : "", + * abs(q) * INT64_C(1000000) / (INT64_C(1) << 31)); + * + * if (fit_last != fit) { + * printk("Finished decoding frame\n"); + * } + * } + * @endcode + */ +typedef uint32_t sensor_frame_iterator_t; + +/** + * @typedef sensor_channel_iterator_t + * @brief Used for iterating over data channels in the same frame via :c:struct:`sensor_decoder_api` + */ +typedef uint32_t sensor_channel_iterator_t; + +/** + * @brief Decodes a single raw data buffer + * + * Data buffers are provided on the :c:struct:`rtio` context that's supplied to + * c:func:`sensor_read`. + */ +struct sensor_decoder_api { + /** + * @brief Get the number of frames in the current buffer. + * + * @param[in] buffer The buffer provided on the :c:struct:`rtio` context. + * @param[out] frame_count The number of frames on the buffer (at least 1) + * @return 0 on success + * @return <0 on error + */ + int (*get_frame_count)(const uint8_t *buffer, uint16_t *frame_count); + + /** + * @brief Get the timestamp associated with the first frame. + * + * @param[in] buffer The buffer provided on the :c:struct:`rtio` context. + * @param[out] timestamp_ns The closest timestamp for when the first frame was generated as + * attained by :c:func:`k_uptime_get`. + * @return 0 on success + * @return <0 on error + */ + int (*get_timestamp)(const uint8_t *buffer, uint64_t *timestamp_ns); + + /** + * @brief Get the scale of a particular channel + * + * This value can be multiplied by the q31_t value to get the SI unit value of the reading. + * It is guaranteed that the scale for a channel will not change between frames. + * + * @param[in] buffer The buffer provided on the :c:struct:`rtio` context. + * @param[in] channel_type The c:enum:`sensor_channel` to query + * @param[out] scale The scale of the channel for this data buffer. + * @return 0 on success + * @return -EINVAL if the @p channel_type doesn't exist in the buffer + * @return <0 on error + */ + int (*get_scale)(const uint8_t *buffer, enum sensor_channel channel_type, uint32_t *scale); + + /** + * @brief Decode up to N samples from the buffer + * + * This function will never wrap frames. If 1 channel is available in the current frame and + * @p max_count is 2, only 1 channel will be decoded and the frame iterator will be modified + * so that the next call to decode will begin at the next frame. + * + * @param[in] buffer The buffer provided on the :c:struct:`rtio` context + * @param[in,out] fit The current frame iterator + * @param[in,out] cit The current channel iterator + * @param[out] channels The channels that were decoded + * @param[out] values The scaled data that was decoded + * @param[in] max_count The maximum number of channels to decode. + * @return + */ + int (*decode)(const uint8_t *buffer, sensor_frame_iterator_t *fit, + sensor_channel_iterator_t *cit, enum sensor_channel *channels, q31_t *values, + uint8_t max_count); +}; + +/** + * @typedef sensor_get_decoder_t + * @brief Get the decoder associate with the given device + * + * @see sensor_get_decoder for more details + */ +typedef int (*sensor_get_decoder_t)(const struct device *dev, const struct sensor_decoder_api **api); + +/* + * Internal data structure used to store information about the IODevice for async reading and + * streaming sensor data. + */ +struct sensor_iodev_data { + const struct device *sensor; + const enum sensor_channel *channels; + size_t num_channels; +}; + +extern struct rtio_iodev_api sensor_iodev_api; + +/** + * @brief Define a reading instance of a sensor + * + * Use this macro to generate a :c:struct:`rtio_iodev` for reading specific channels. Example: + * + * @code(.c) + * SENSOR_DT_IODEV_DEFINE(icm42688_accelgyro, DT_NODELABEL(icm42688), + * {SENSOR_CHAN_ACCEL_XYZ, SENSOR_CHAN_GYRO_XYZ}); + * + * int main(void) { + * sensor_read(&icm42688_accelgyro, &rtio); + * } + * @endcode + */ +#define SENSOR_DT_IODEV_DEFINE(name, dt_node, ...) \ + const enum sensor_channel _channel_array_##name[] = {__VA_ARGS__}; \ + const struct sensor_iodev_data _iodev_data_##name = { \ + .sensor = DEVICE_DT_GET(dt_node), \ + .channels = _channel_array_##name, \ + .num_channels = ARRAY_SIZE(_channel_array_##name), \ + }; \ + RTIO_IODEV_DEFINE(name, &sensor_iodev_api, (void *)&_iodev_data_##name) + +/** + * @typedef sensor_submit_t + * @brief Trigger a one-shot read on the sensor + * + * @see sensor_submit for more details + */ +typedef int (*sensor_submit_t)(const struct device *sensor, struct rtio_iodev_sqe *sqe); + +/* The default decoder API */ +extern struct sensor_decoder_api __sensor_default_decoder; + __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; + sensor_get_decoder_t get_decoder; + sensor_submit_t submit; }; /** @@ -598,6 +761,54 @@ static inline int z_impl_sensor_channel_get(const struct device *dev, return api->channel_get(dev, chan, val); } +struct sensor_data_generic_channel { + uint32_t scale; + enum sensor_channel channel; +}; + +struct sensor_data_generic_header { + uint64_t timestamp_ns; + size_t num_channels; + struct sensor_data_generic_channel channel_info[0]; +}; + +#define SENSOR_CHANNEL_3_AXIS(chan) \ + ((chan) == SENSOR_CHAN_ACCEL_XYZ || (chan) == SENSOR_CHAN_GYRO_XYZ || \ + (chan) == SENSOR_CHAN_MAGN_XYZ) + +__syscall int sensor_get_decoder(const struct device *dev, const struct sensor_decoder_api **decoder); + +static inline int z_impl_sensor_get_decoder(const struct device *dev, const struct sensor_decoder_api **decoder) +{ + const struct sensor_driver_api *api = dev->api; + + __ASSERT_NO_MSG(api != NULL); + + if (api->get_decoder == NULL) { + *decoder = &__sensor_default_decoder; + return 0; + } + + return api->get_decoder(dev, decoder); +} + +static inline int sensor_read(const struct rtio_iodev *dev, struct rtio *ctx) +{ + struct rtio_sqe *sqe = rtio_sqe_acquire(ctx); + const struct sensor_iodev_data *data = dev->data; + + if (sqe == NULL) { + return -ENOMEM; + } + rtio_sqe_prep_read_with_pool(sqe, dev, 0, (void *)data->sensor); + rtio_submit(ctx, 0); + return 0; +} + +typedef void (*sensor_processing_callback_t)(const struct device *sensor, int result, uint8_t *buf, + uint32_t buf_len); +void z_sensor_processing_loop(struct rtio *ctx, sensor_processing_callback_t cb); + /** * @brief The value of gravitational constant in micro m/s^2. */ diff --git a/include/zephyr/rtio/rtio.h b/include/zephyr/rtio/rtio.h index d5f3cdbbb82b1b..919a01dd8434e0 100644 --- a/include/zephyr/rtio/rtio.h +++ b/include/zephyr/rtio/rtio.h @@ -333,18 +333,6 @@ enum rtio_mempool_entry_state { /* Check that we can always fit the state in 2 bits */ BUILD_ASSERT(RTIO_MEMPOOL_ENTRY_STATE_COUNT < 4); -/** - * @brief An entry mapping a mempool entry to its sqe. - */ -struct rtio_mempool_map_entry { - /** The state of the sqe map entry */ - enum rtio_mempool_entry_state state : 2; - /** The starting block index into the mempool buffer */ - uint16_t block_idx : 15; - /** Number of blocks after the block_idx */ - uint16_t block_count : 15; -}; - /** * @brief An RTIO queue pair that both the kernel and application work with * @@ -395,8 +383,6 @@ struct rtio { struct sys_mem_blocks *mempool; /* The size (in bytes) of a single block in the mempool */ uint32_t mempool_blk_size; - /* Map of allocated starting blocks */ - struct rtio_mempool_map_entry *mempool_map; #endif /* CONFIG_RTIO_SYS_MEM_BLOCKS */ }; @@ -660,7 +646,7 @@ static inline void rtio_sqe_prep_transceive(struct rtio_sqe *sqe, RTIO_SQ_DEFINE(_sq_##name, sq_sz); \ RTIO_CQ_DEFINE(_cq_##name, cq_sz); \ STRUCT_SECTION_ITERABLE(rtio, name) = { \ - .executor = (exec), \ + .executor = (struct rtio_executor *)(exec), \ IF_ENABLED(CONFIG_RTIO_SUBMIT_SEM, (.submit_sem = &_submit_sem_##name,)) \ IF_ENABLED(CONFIG_RTIO_SUBMIT_SEM, (.submit_count = 0,)) \ IF_ENABLED(CONFIG_RTIO_CONSUME_SEM, (.consume_sem = &_consume_sem_##name,)) \ @@ -722,11 +708,9 @@ static inline void rtio_sqe_prep_transceive(struct rtio_sqe *sqe, _mempool_buf_##name[num_blks*WB_UP(blk_size)]; \ _SYS_MEM_BLOCKS_DEFINE_WITH_EXT_BUF(_mempool_##name, WB_UP(blk_size), num_blks, \ _mempool_buf_##name, RTIO_DMEM); \ - RTIO_BMEM struct rtio_mempool_map_entry _mempool_map_##name[sq_sz]; \ _RTIO_DEFINE(name, exec, sq_sz, cq_sz) \ .mempool = &_mempool_##name, \ .mempool_blk_size = WB_UP(blk_size), \ - .mempool_map = _mempool_map_##name, \ } /* clang-format on */ @@ -887,17 +871,12 @@ static inline uint32_t rtio_cqe_compute_flags(struct rtio_iodev_sqe *iodev_sqe) #ifdef CONFIG_RTIO_SYS_MEM_BLOCKS if (iodev_sqe->sqe->op == RTIO_OP_RX && iodev_sqe->sqe->flags & RTIO_SQE_MEMPOOL_BUFFER) { struct rtio *r = iodev_sqe->r; - int sqe_index = iodev_sqe->sqe - r->sq->buffer; - struct rtio_mempool_map_entry *map_entry = &r->mempool_map[sqe_index]; + int blk_index = (iodev_sqe->sqe->buf - r->mempool->buffer) / r->mempool_blk_size; + int blk_count = iodev_sqe->sqe->buf_len / r->mempool_blk_size; - if (map_entry->state != RTIO_MEMPOOL_ENTRY_STATE_ALLOCATED) { - /* Not allocated to this sqe */ - return flags; - } - - flags |= RTIO_CQE_FLAG_MEMPOOL_BUFFER; - flags |= RTIO_CQE_FLAG_PREP_VALUE(sqe_index); - map_entry->state = RTIO_MEMPOOL_ENTRY_STATE_ZOMBIE; + flags = FIELD_PREP(GENMASK(7, 0), RTIO_CQE_FLAG_MEMPOOL_BUFFER) | + FIELD_PREP(GENMASK(19, 8), blk_index) | + FIELD_PREP(GENMASK(31, 20), blk_count); } #endif @@ -926,15 +905,12 @@ static inline int z_impl_rtio_cqe_get_mempool_buffer(const struct rtio *r, struc uint8_t **buff, uint32_t *buff_len) { #ifdef CONFIG_RTIO_SYS_MEM_BLOCKS - if (cqe->flags & RTIO_CQE_FLAG_MEMPOOL_BUFFER) { - int map_idx = RTIO_CQE_FLAG_GET_VALUE(cqe->flags); - struct rtio_mempool_map_entry *entry = &r->mempool_map[map_idx]; + if (FIELD_GET(GENMASK(7, 0), cqe->flags) == RTIO_CQE_FLAG_MEMPOOL_BUFFER) { + int blk_idx = FIELD_GET(GENMASK(19, 8), cqe->flags); + int blk_count = FIELD_GET(GENMASK(31, 20), cqe->flags); - if (entry->state != RTIO_MEMPOOL_ENTRY_STATE_ZOMBIE) { - return -EINVAL; - } - *buff = r->mempool->buffer + entry->block_idx * r->mempool_blk_size; - *buff_len = entry->block_count * r->mempool_blk_size; + *buff = r->mempool->buffer + blk_idx * r->mempool_blk_size; + *buff_len = blk_count * r->mempool_blk_size; __ASSERT_NO_MSG(*buff >= r->mempool->buffer); __ASSERT_NO_MSG(*buff < r->mempool->buffer + r->mempool_blk_size * r->mempool->num_blocks); @@ -1058,14 +1034,14 @@ static inline int rtio_sqe_rx_buf(const struct rtio_iodev_sqe *iodev_sqe, uint32 struct sys_mem_blocks *pool = r->mempool; uint32_t bytes = max_buf_len; int sqe_index = sqe - r->sq->buffer; - struct rtio_mempool_map_entry *map_entry = &r->mempool_map[sqe_index]; + struct rtio_sqe *mutable_sqe = &r->sq->buffer[sqe_index]; - if (map_entry->state == RTIO_MEMPOOL_ENTRY_STATE_ALLOCATED) { - if (map_entry->block_count * blk_size < min_buf_len) { + if (sqe->buf != NULL) { + if (sqe->buf_len < min_buf_len) { return -ENOMEM; } - *buf = &r->mempool->buffer[map_entry->block_count * blk_size]; - *buf_len = map_entry->block_count * blk_size; + *buf = sqe->buf; + *buf_len = sqe->buf_len; return 0; } @@ -1075,9 +1051,8 @@ static inline int rtio_sqe_rx_buf(const struct rtio_iodev_sqe *iodev_sqe, uint32 if (rc == 0) { *buf_len = num_blks * blk_size; - map_entry->state = RTIO_MEMPOOL_ENTRY_STATE_ALLOCATED; - map_entry->block_idx = __rtio_compute_mempool_block_index(r, *buf); - map_entry->block_count = num_blks; + mutable_sqe->buf = *buf; + mutable_sqe->buf_len = *buf_len; return 0; } if (bytes == min_buf_len) { @@ -1124,20 +1099,6 @@ static inline void z_impl_rtio_release_buffer(struct rtio *r, void *buff) if (rc != 0) { return; } - - uint16_t blk_index = __rtio_compute_mempool_block_index(r, buff); - - if (blk_index == UINT16_MAX) { - return; - } - for (unsigned long i = 0; i < r->sq->_spsc.mask + 1; ++i) { - struct rtio_mempool_map_entry *entry = &r->mempool_map[i]; - - if (entry->block_idx == blk_index) { - entry->state = RTIO_MEMPOOL_ENTRY_STATE_FREE; - break; - } - } #endif } diff --git a/samples/sensor/sensor_shell/prj.conf b/samples/sensor/sensor_shell/prj.conf index 2a9e6a34221698..92128737a9c947 100644 --- a/samples/sensor/sensor_shell/prj.conf +++ b/samples/sensor/sensor_shell/prj.conf @@ -4,4 +4,12 @@ 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_DSP=y +CONFIG_CMSIS_DSP=y diff --git a/samples/sensor/sensor_shell/src/main.c b/samples/sensor/sensor_shell/src/main.c index 235cc58982cd4f..fc790970fdb6d9 100644 --- a/samples/sensor/sensor_shell/src/main.c +++ b/samples/sensor/sensor_shell/src/main.c @@ -6,11 +6,90 @@ #include #include +#include #include #include +#include +#include + +#define LOOP_DURATION_MS 500 +#define BUSY_WAIT_DURATION_US 1 + LOG_MODULE_REGISTER(app); +static const struct device *accel0_dev = DEVICE_DT_GET(DT_ALIAS(accel0)); + +SENSOR_DT_IODEV_DEFINE(accel0, DT_ALIAS(accel0), SENSOR_CHAN_ACCEL_XYZ, SENSOR_CHAN_GYRO_XYZ); +SENSOR_DT_IODEV_DEFINE(magn0, DT_ALIAS(magn0), SENSOR_CHAN_MAGN_XYZ); + +RTIO_EXECUTOR_SIMPLE_DEFINE(sensor_executor); +RTIO_DEFINE_WITH_MEMPOOL(rtio_ctx, &sensor_executor, 512, 512, 512, 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, ×tamp_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; + } + // printk("%p decoded %d channels\n", buf, count); + 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++; + // printk("q=%s0.%06" PRIi64 "\n", q[0] < 0 ? "-" : "", + // abs(q[0]) * INT64_C(1000000) / (INT64_C(1) << 31)); + } +} + +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, 0, 0, 0); + enum sample_stats_state { SAMPLE_STATS_STATE_UNINITIALIZED = 0, SAMPLE_STATS_STATE_ENABLED, @@ -76,13 +155,109 @@ static void data_ready_trigger_handler(const struct device *sensor, 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); + // 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); + // } + + 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 end_ts; + uint64_t start_ts = k_uptime_get(); + 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); + } + } 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_requests - failed_reads)) / (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); + + num_requests = 0; + failed_reads = 0; + num_samples = 0; + + start_ts = k_uptime_get(); + do { + num_requests++; + int rc = sensor_sample_fetch(accel0_dev); + + 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("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_requests - failed_reads)) / (duration * 1000)); + printk("Num Requests: %d\n", num_requests); + printk("Num Samples: %d\n", num_samples); + return 0; }