-
Notifications
You must be signed in to change notification settings - Fork 6.6k
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
- Loading branch information
Showing
6 changed files
with
432 additions
and
71 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,212 @@ | ||
| // | ||
| // Created by peress on 14/04/23. | ||
| // | ||
|
|
||
| #include <errno.h> | ||
|
|
||
| #include <zephyr/drivers/sensor.h> | ||
| #include <zephyr/dsp/dsp.h> | ||
| #include <zephyr/logging/log.h> | ||
|
|
||
| LOG_MODULE_REGISTER(rtio_sensor, CONFIG_SENSOR_LOG_LEVEL); | ||
|
|
||
| int __sensor_read_fallback(const struct device *dev, struct rtio *ctx, | ||
| enum sensor_channel *channels, size_t num_channels) | ||
| { | ||
|
|
||
| uint64_t timestamp_ns = k_uptime_get(); | ||
| int rc = sensor_sample_fetch(dev); | ||
| if (rc != 0) { | ||
| return rc; | ||
| } | ||
|
|
||
| LOG_DBG("reading %d channels", (int)num_channels); | ||
| for (size_t i = 0; i < num_channels; ++i) { | ||
| struct sensor_value value[3]; | ||
| struct rtio_sqe *sqe = rtio_sqe_acquire(ctx); | ||
| struct rtio_iodev_sqe iodev_sqe = { | ||
| .sqe = sqe, | ||
| .r = ctx, | ||
| }; | ||
| int num_samples = SENSOR_CHANNEL_3_AXIS(channels[i]) ? 3 : 1; | ||
| uint32_t min_buf_len = | ||
| sizeof(struct sensor_data_generic_header) + num_channels * sizeof(q31_t); | ||
| uint8_t *buf; | ||
| uint32_t buf_len; | ||
|
|
||
| if (sqe == NULL) { | ||
| LOG_DBG("Failed to get sqe"); | ||
| rtio_sqe_drop_all(ctx); | ||
| return -ENOMEM; | ||
| } | ||
| sqe->op = RTIO_OP_RX; | ||
| sqe->flags = RTIO_SQE_MEMPOOL_BUFFER; | ||
| sqe->buf = NULL; | ||
| sqe->buf_len = 0; | ||
| LOG_DBG(" [%d] num_samples=%d", (int)i, num_samples); | ||
| LOG_DBG(" [%d] requesting buffer of length %u bytes", (int)i, min_buf_len); | ||
| rc = rtio_sqe_rx_buf(&iodev_sqe, min_buf_len, min_buf_len, &buf, &buf_len); | ||
| if (rc != 0) { | ||
| /* Failed to allocate read buffer */ | ||
| LOG_DBG(" [%d] failed to allocate memory", (int)i); | ||
| rtio_cqe_submit(ctx, rc, (void *)dev, 0); | ||
| rtio_sqe_drop_all(ctx); | ||
| return rc; | ||
| } | ||
| LOG_DBG(" [%d] writing to %p", (int)i, buf); | ||
| rc = sensor_channel_get(dev, channels[i], value); | ||
| if (rc != 0) { | ||
| LOG_DBG(" [%d] failed to get channel %d", (int)i, channels[i]); | ||
| rtio_cqe_submit(ctx, rc, (void *)dev, 0); | ||
| rtio_sqe_drop_all(ctx); | ||
| return rc; | ||
| } | ||
|
|
||
| /* Package the data */ | ||
| struct sensor_data_generic_header *header = | ||
| (struct sensor_data_generic_header *)buf; | ||
| header->timestamp_ns = timestamp_ns; | ||
| header->channel = channels[i]; | ||
| for (int sample = 0; sample < num_samples; ++sample) { | ||
| uint32_t scale = (uint32_t)llabs((int64_t)value[sample].val1 + 1); | ||
|
|
||
| if (sample == 0 || header->scale < scale) { | ||
| header->scale = scale; | ||
| } | ||
| } | ||
| LOG_DBG(" [%d] scale=%u", (int)i, header->scale); | ||
| for (int sample = 0; sample < num_samples; ++sample) { | ||
| q31_t *q = (q31_t *)(buf + sizeof(struct sensor_data_generic_header) + | ||
| sizeof(q31_t) * sample); | ||
|
|
||
| int64_t value_u = | ||
| value[sample].val1 * INT64_C(1000000) + value[sample].val2; | ||
| *q = (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); | ||
| } | ||
|
|
||
| uint32_t flags = rtio_cqe_compute_flags(&iodev_sqe); | ||
| LOG_DBG(" [%d] flags=0x%08x", (int)i, flags); | ||
| rtio_cqe_submit(ctx, rc, (void *)dev, flags); | ||
| rtio_sqe_drop_all(ctx); | ||
| } | ||
| return 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 int get_scale(const uint8_t *buffer, uint32_t channel_type, uint32_t *scale) | ||
| { | ||
| struct sensor_data_generic_header *header = (struct sensor_data_generic_header *)buffer; | ||
|
|
||
| switch (header->channel) { | ||
| case SENSOR_CHAN_ACCEL_XYZ: | ||
| if (channel_type != SENSOR_CHAN_ACCEL_X && channel_type != SENSOR_CHAN_ACCEL_Y && | ||
| channel_type != SENSOR_CHAN_ACCEL_Z) { | ||
| return -EINVAL; | ||
| } | ||
| break; | ||
| case SENSOR_CHAN_GYRO_XYZ: | ||
| if (channel_type != SENSOR_CHAN_GYRO_X && channel_type != SENSOR_CHAN_GYRO_Y && | ||
| channel_type != SENSOR_CHAN_GYRO_Z) { | ||
| return -EINVAL; | ||
| } | ||
| break; | ||
| case SENSOR_CHAN_MAGN_XYZ: | ||
| if (channel_type != SENSOR_CHAN_MAGN_X && channel_type != SENSOR_CHAN_MAGN_Y && | ||
| channel_type != SENSOR_CHAN_MAGN_Z) { | ||
| return -EINVAL; | ||
| } | ||
| break; | ||
| default: | ||
| if (header->channel != channel_type) { | ||
| return -EINVAL; | ||
| } | ||
| } | ||
| *scale = header->scale; | ||
| return 0; | ||
| } | ||
|
|
||
| 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)); | ||
|
|
||
| if (*fit != 0) { | ||
| LOG_ERR("Frame iterator must be 0"); | ||
| return -EINVAL; | ||
| } | ||
|
|
||
| if (!SENSOR_CHANNEL_3_AXIS(header->channel)) { | ||
| /* Frame contains only 1 data field */ | ||
| if (*cit != 0) { | ||
| LOG_ERR("Channel iterator must be 0"); | ||
| return -EINVAL; | ||
| } | ||
| channels[0] = header->channel; | ||
| values[0] = q[0]; | ||
| *fit = 1; | ||
| return 1; | ||
| } | ||
|
|
||
| /* 3 Axis data */ | ||
| int i = 0; | ||
| sensor_channel_iterator_t end = MIN(3, *cit + max_count); | ||
| LOG_DBG("Reading from cit %" PRIu32 " to %" PRIu32, *cit, end); | ||
| for (; i + *cit < end; ++i) { | ||
| channels[i] = header->channel - 3 + i + *cit; | ||
| values[i] = q[i + *cit]; | ||
| } | ||
| if (i + *cit == 3) { | ||
| /* Wrap to next frame */ | ||
| *fit = 1; | ||
| *cit = 0; | ||
| } else { | ||
| *cit += i; | ||
| } | ||
| return i; | ||
| } | ||
|
|
||
| struct sensor_decoder_api __sensor_default_decoder = { | ||
| .get_frame_count = get_frame_count, | ||
| .get_timestamp = get_timestamp, | ||
| .get_scale = get_scale, | ||
| .decode = decode, | ||
| }; |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.