sensors: Add new async one-shot reading API
Add a new async API based on the RTIO subsystem. This new API allows: 1. Users to create sampling configs (telling the sensor which channels they want to sample together). 2. Sample data in an asynchronous manner which provides greater control over the data processing priority. 3. Fully backwards compatible API with no driver changes needed for functionality (they are needed to improve performance). 4. Helper functions for processing loop. Signed-off-by: Yuval Peress <peress@google.com>
This commit is contained in:
Yuval Peress
Maureen Helm
parent
9c624fa9e6
commit
ed380de152
|
|
@ -140,6 +140,10 @@ if(CONFIG_SENSOR_INFO)
|
|||
zephyr_iterable_section(NAME sensor_info KVMA RAM_REGION GROUP RODATA_REGION SUBALIGN 4)
|
||||
endif()
|
||||
|
||||
if(CONFIG_SENSOR_ASYNC_API)
|
||||
zephyr_iterable_section(NAME sensor_decoder_api KVMA RAM_REGION GROUP RODATA_REGION SUBALIGN 4)
|
||||
endif()
|
||||
|
||||
if(CONFIG_MCUMGR)
|
||||
zephyr_iterable_section(NAME mcumgr_handler KVMA RAM_REGION GROUP RODATA_REGION SUBALIGN 4)
|
||||
endif()
|
||||
|
|
|
|||
|
|
@ -148,3 +148,4 @@ zephyr_library_property(ALLOW_EMPTY TRUE)
|
|||
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)
|
||||
zephyr_library_sources_ifdef(CONFIG_SENSOR_ASYNC_API sensor_decoders_init.c default_rtio_sensor.c)
|
||||
|
|
|
|||
|
|
@ -20,6 +20,13 @@ config SENSOR_INIT_PRIORITY
|
|||
help
|
||||
Sensor initialization priority.
|
||||
|
||||
config SENSOR_ASYNC_API
|
||||
bool "Async Sensor API"
|
||||
select RTIO
|
||||
select RTIO_SYS_MEM_BLOCKS
|
||||
help
|
||||
Enables the asynchronous sensor API by leveraging the RTIO subsystem.
|
||||
|
||||
config SENSOR_SHELL
|
||||
bool "Sensor shell"
|
||||
depends on SHELL
|
||||
|
|
|
|||
350
drivers/sensor/default_rtio_sensor.c
Normal file
350
drivers/sensor/default_rtio_sensor.c
Normal file
|
|
@ -0,0 +1,350 @@
|
|||
/*
|
||||
* Copyright (c) 2023 Google LLC.
|
||||
*
|
||||
* SPDX-License-Identifier: Apache-2.0
|
||||
*/
|
||||
|
||||
#include <errno.h>
|
||||
|
||||
#include <zephyr/drivers/sensor.h>
|
||||
#include <zephyr/dsp/types.h>
|
||||
#include <zephyr/logging/log.h>
|
||||
|
||||
LOG_MODULE_REGISTER(sensor_compat, 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_read_config *cfg = iodev_sqe->sqe.iodev->data;
|
||||
const struct device *dev = cfg->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);
|
||||
}
|
||||
}
|
||||
|
||||
const struct rtio_iodev_api __sensor_iodev_api = {
|
||||
.submit = sensor_iodev_submit,
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Compute the number of samples needed for the given channels
|
||||
*
|
||||
* @param[in] channels Array of channels requested
|
||||
* @param[in] num_channels Number of channels on the @p channels array
|
||||
* @return The number of samples required to read the given channels
|
||||
*/
|
||||
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;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Compute the minimum number of bytes needed
|
||||
*
|
||||
* @param[in] num_output_samples The number of samples to represent
|
||||
* @return The number of bytes needed for this sample frame
|
||||
*/
|
||||
static inline uint32_t compute_min_buf_len(int num_output_samples)
|
||||
{
|
||||
return sizeof(struct sensor_data_generic_header) + (num_output_samples * sizeof(q31_t)) +
|
||||
(num_output_samples * sizeof(enum sensor_channel));
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Checks if the header already contains a given channel
|
||||
*
|
||||
* @param[in] header The header to scan
|
||||
* @param[in] channel The channel to search for
|
||||
* @param[in] num_channels The number of valid channels in the header so far
|
||||
* @return Index of the @p channel if found or negative if not found
|
||||
*/
|
||||
static inline int check_header_contains_channel(const struct sensor_data_generic_header *header,
|
||||
enum sensor_channel channel, int num_channels)
|
||||
{
|
||||
__ASSERT_NO_MSG(!SENSOR_CHANNEL_3_AXIS(channel));
|
||||
|
||||
for (int i = 0; i < num_channels; ++i) {
|
||||
if (header->channels[i] == channel) {
|
||||
return i;
|
||||
}
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Fallback function for retrofiting old drivers to rtio
|
||||
*
|
||||
* @param[in] dev The sensor device to read
|
||||
* @param[in] iodev_sqe The read submission queue event
|
||||
*/
|
||||
static void sensor_submit_fallback(const struct device *dev, struct rtio_iodev_sqe *iodev_sqe)
|
||||
{
|
||||
const struct sensor_read_config *cfg = iodev_sqe->sqe.iodev->data;
|
||||
const enum sensor_channel *const channels = cfg->channels;
|
||||
const int num_output_samples = compute_num_samples(channels, cfg->count);
|
||||
uint32_t min_buf_len = compute_min_buf_len(num_output_samples);
|
||||
uint64_t timestamp_ns = k_ticks_to_ns_floor64(k_uptime_ticks());
|
||||
int rc = sensor_sample_fetch(dev);
|
||||
uint8_t *buf;
|
||||
uint32_t buf_len;
|
||||
|
||||
/* Check that the fetch succeeded */
|
||||
if (rc != 0) {
|
||||
LOG_ERR("Failed to fetch samples");
|
||||
rtio_iodev_sqe_err(iodev_sqe, rc);
|
||||
return;
|
||||
}
|
||||
|
||||
/* Get the buffer for the frame, it may be allocated dynamically by the rtio context */
|
||||
rc = rtio_sqe_rx_buf(iodev_sqe, min_buf_len, min_buf_len, &buf, &buf_len);
|
||||
if (rc != 0) {
|
||||
LOG_ERR("Failed to get a read buffer of size %u bytes", min_buf_len);
|
||||
rtio_iodev_sqe_err(iodev_sqe, rc);
|
||||
return;
|
||||
}
|
||||
|
||||
/* Set the timestamp and 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;
|
||||
header->shift = 0;
|
||||
|
||||
q31_t *q = (q31_t *)(buf + sizeof(struct sensor_data_generic_header) +
|
||||
num_output_samples * sizeof(enum sensor_channel));
|
||||
|
||||
/* Populate values, update shift, and set channels */
|
||||
for (size_t i = 0, sample_idx = 0; i < cfg->count; ++i) {
|
||||
struct sensor_value value[3];
|
||||
const int num_samples = SENSOR_CHANNEL_3_AXIS(channels[i]) ? 3 : 1;
|
||||
|
||||
/* Get the current channel requested by the user */
|
||||
rc = sensor_channel_get(dev, channels[i], value);
|
||||
|
||||
if (num_samples == 3) {
|
||||
header->channels[sample_idx++] =
|
||||
rc == 0 ? channels[i] - 3 : SENSOR_CHAN_MAX;
|
||||
header->channels[sample_idx++] =
|
||||
rc == 0 ? channels[i] - 2 : SENSOR_CHAN_MAX;
|
||||
header->channels[sample_idx++] =
|
||||
rc == 0 ? channels[i] - 1 : SENSOR_CHAN_MAX;
|
||||
} else {
|
||||
header->channels[sample_idx++] = rc == 0 ? channels[i] : SENSOR_CHAN_MAX;
|
||||
}
|
||||
|
||||
if (rc != 0) {
|
||||
LOG_DBG("Failed to get channel %d, skipping", channels[i]);
|
||||
continue;
|
||||
}
|
||||
|
||||
/* Get the largest absolute value reading to set the scale for the channel */
|
||||
uint32_t header_scale = 0;
|
||||
|
||||
for (int sample = 0; sample < num_samples; ++sample) {
|
||||
/*
|
||||
* The scale is the ceil(abs(sample)).
|
||||
* Since we are using fractional values, it's easier to assume that .val2
|
||||
* is non 0 and convert this to abs(sample.val1) + 1 (removing a branch).
|
||||
* Since it's possible that val1 (int32_t) is saturated (INT32_MAX) we need
|
||||
* to upcast it to 64 bit int first, then take the abs() of that 64 bit
|
||||
* int before we '+ 1'. Once that's done, we can safely cast back down
|
||||
* to uint32_t because the min value is 0 and max is INT32_MAX + 1 which
|
||||
* is less than UINT32_MAX.
|
||||
*/
|
||||
uint32_t scale = (uint32_t)llabs((int64_t)value[sample].val1) + 1;
|
||||
|
||||
header_scale = MAX(header_scale, scale);
|
||||
}
|
||||
|
||||
int8_t new_shift = ilog2(header_scale - 1) + 1;
|
||||
|
||||
/* Reset sample_idx */
|
||||
sample_idx -= num_samples;
|
||||
if (header->shift < new_shift) {
|
||||
/*
|
||||
* Shift was updated, need to convert all the existing q values. This could
|
||||
* be optimized by calling zdsp_scale_q31() but that would force a
|
||||
* dependency between sensors and the zDSP subsystem.
|
||||
*/
|
||||
for (int q_idx = 0; q_idx < sample_idx; ++q_idx) {
|
||||
q[q_idx] = q[q_idx] >> (new_shift - header->shift);
|
||||
}
|
||||
header->shift = new_shift;
|
||||
}
|
||||
|
||||
/*
|
||||
* Spread the q31 values. This is needed because some channels are 3D. If
|
||||
* the user specified one of those then num_samples will be 3; and we need to
|
||||
* produce 3 separate readings.
|
||||
*/
|
||||
for (int sample = 0; sample < num_samples; ++sample) {
|
||||
/* Check if the channel is already in the buffer */
|
||||
int prev_computed_value_idx = check_header_contains_channel(
|
||||
header, header->channels[sample_idx + sample], sample_idx + sample);
|
||||
|
||||
if (prev_computed_value_idx >= 0 &&
|
||||
prev_computed_value_idx != sample_idx + sample) {
|
||||
LOG_DBG("value[%d] previously computed at q[%d]@%p", sample,
|
||||
prev_computed_value_idx,
|
||||
(void *)&q[prev_computed_value_idx]);
|
||||
q[sample_idx + sample] = q[prev_computed_value_idx];
|
||||
continue;
|
||||
}
|
||||
|
||||
/* Convert the value to micro-units */
|
||||
int64_t value_u = sensor_value_to_micro(&value[sample]);
|
||||
|
||||
/* Convert to q31 using the shift */
|
||||
q[sample_idx + sample] =
|
||||
((value_u * ((INT64_C(1) << 31) - 1)) / 1000000) >> header->shift;
|
||||
|
||||
LOG_DBG("value[%d]=%s%d.%06d, q[%d]@%p=%d", sample, value_u < 0 ? "-" : "",
|
||||
abs((int)value[sample].val1), abs((int)value[sample].val2),
|
||||
(int)(sample_idx + sample), (void *)&q[sample_idx + sample],
|
||||
q[sample_idx + sample]);
|
||||
}
|
||||
sample_idx += num_samples;
|
||||
}
|
||||
LOG_DBG("Total channels in header: %zu", header->num_channels);
|
||||
rtio_iodev_sqe_ok(iodev_sqe, 0);
|
||||
}
|
||||
|
||||
void sensor_processing_with_callback(struct rtio *ctx, sensor_processing_callback_t cb)
|
||||
{
|
||||
void *userdata = NULL;
|
||||
uint8_t *buf = NULL;
|
||||
uint32_t buf_len = 0;
|
||||
int rc;
|
||||
|
||||
/* Wait for a CQE */
|
||||
struct rtio_cqe *cqe = rtio_cqe_consume_block(ctx);
|
||||
|
||||
/* Cache the data from the CQE */
|
||||
rc = cqe->result;
|
||||
userdata = cqe->userdata;
|
||||
rtio_cqe_get_mempool_buffer(ctx, cqe, &buf, &buf_len);
|
||||
|
||||
/* Release the CQE */
|
||||
rtio_cqe_release(ctx, cqe);
|
||||
|
||||
/* Call the callback */
|
||||
cb(rc, buf, buf_len, userdata);
|
||||
|
||||
/* Release the memory */
|
||||
rtio_release_buffer(ctx, buf, buf_len);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Default decoder get frame count
|
||||
*
|
||||
* Default reader can only ever service a single frame at a time.
|
||||
*
|
||||
* @param[in] buffer The data buffer to parse
|
||||
* @param[out] frame_count The number of frames in the buffer (always 1)
|
||||
* @return 0 in all cases
|
||||
*/
|
||||
static int get_frame_count(const uint8_t *buffer, uint16_t *frame_count)
|
||||
{
|
||||
ARG_UNUSED(buffer);
|
||||
*frame_count = 1;
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Default decoder get the timestamp of the first frame
|
||||
*
|
||||
* @param[in] buffer The data buffer to parse
|
||||
* @param[out] timestamp_ns The timestamp of the first frame
|
||||
* @return 0 in all cases
|
||||
*/
|
||||
static int get_timestamp(const uint8_t *buffer, uint64_t *timestamp_ns)
|
||||
{
|
||||
*timestamp_ns = ((struct sensor_data_generic_header *)buffer)->timestamp_ns;
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Default decoder get the bitshift of the given channel (if possible)
|
||||
*
|
||||
* @param[in] buffer The data buffer to parse
|
||||
* @param[in] channel_type The channel to query
|
||||
* @param[out] shift The bitshift for the q31 value
|
||||
* @return 0 on success
|
||||
* @return -EINVAL if the @p channel_type couldn't be found
|
||||
*/
|
||||
static int get_shift(const uint8_t *buffer, enum sensor_channel channel_type, int8_t *shift)
|
||||
{
|
||||
struct sensor_data_generic_header *header = (struct sensor_data_generic_header *)buffer;
|
||||
|
||||
ARG_UNUSED(channel_type);
|
||||
*shift = header->shift;
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Default decoder decode N samples
|
||||
*
|
||||
* Decode up to N samples starting at the provided @p fit and @p cit. The appropriate channel types
|
||||
* and q31 values will be placed in @p values and @p channels respectively.
|
||||
*
|
||||
* @param[in] buffer The data buffer to decode
|
||||
* @param[in,out] fit The starting frame iterator
|
||||
* @param[in,out] cit The starting channel iterator
|
||||
* @param[out] channels The decoded channel types
|
||||
* @param[out] values The decoded q31 values
|
||||
* @param[in] max_count The maximum number of values to decode
|
||||
* @return > 0 The number of decoded values
|
||||
* @return 0 Nothing else to decode on this @p buffer
|
||||
* @return < 0 Error
|
||||
*/
|
||||
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(enum sensor_channel));
|
||||
int count = 0;
|
||||
|
||||
if (*fit != 0 || *cit >= header->num_channels) {
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
/* Skip invalid channels */
|
||||
while (*cit < header->num_channels && header->channels[*cit] == SENSOR_CHAN_MAX) {
|
||||
*cit += 1;
|
||||
}
|
||||
|
||||
for (; *cit < header->num_channels && count < max_count; ++count) {
|
||||
channels[count] = header->channels[*cit];
|
||||
values[count] = q[*cit];
|
||||
LOG_DBG("Decoding q[%u]@%p=%d", *cit, (void *)&q[*cit], q[*cit]);
|
||||
*cit += 1;
|
||||
}
|
||||
|
||||
if (*cit >= header->num_channels) {
|
||||
*fit = 1;
|
||||
*cit = 0;
|
||||
}
|
||||
return count;
|
||||
}
|
||||
|
||||
const struct sensor_decoder_api __sensor_default_decoder = {
|
||||
.get_frame_count = get_frame_count,
|
||||
.get_timestamp = get_timestamp,
|
||||
.get_shift = get_shift,
|
||||
.decode = decode,
|
||||
};
|
||||
18
drivers/sensor/sensor_decoders_init.c
Normal file
18
drivers/sensor/sensor_decoders_init.c
Normal file
|
|
@ -0,0 +1,18 @@
|
|||
/*
|
||||
* Copyright (c) 2023 Google LLC.
|
||||
*
|
||||
* SPDX-License-Identifier: Apache-2.0
|
||||
*/
|
||||
|
||||
#include <zephyr/init.h>
|
||||
#include <zephyr/drivers/sensor.h>
|
||||
|
||||
static int sensor_decoders_init(void)
|
||||
{
|
||||
STRUCT_SECTION_FOREACH(sensor_decoder_api, api) {
|
||||
k_object_access_all_grant(api);
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
SYS_INIT(sensor_decoders_init, POST_KERNEL, 99);
|
||||
|
|
@ -57,3 +57,26 @@ static inline int z_vrfy_sensor_channel_get(const struct device *dev,
|
|||
(struct sensor_value *)val);
|
||||
}
|
||||
#include <syscalls/sensor_channel_get_mrsh.c>
|
||||
|
||||
#ifdef CONFIG_SENSOR_ASYNC_API
|
||||
static inline int z_vrfy_sensor_get_decoder(const struct device *dev,
|
||||
const struct sensor_decoder_api **decoder)
|
||||
{
|
||||
Z_OOPS(Z_SYSCALL_OBJ(dev, K_OBJ_DRIVER_SENSOR));
|
||||
Z_OOPS(Z_SYSCALL_MEMORY_READ(decoder, sizeof(struct sensor_decoder_api)));
|
||||
return z_impl_sensor_get_decoder(dev, decoder);
|
||||
}
|
||||
#include <syscalls/sensor_get_decoder_mrsh.c>
|
||||
|
||||
static inline int z_vrfy_sensor_reconfigure_read_iodev(struct rtio_iodev *iodev,
|
||||
const struct device *sensor,
|
||||
const enum sensor_channel *channels,
|
||||
size_t num_channels)
|
||||
{
|
||||
Z_OOPS(Z_SYSCALL_OBJ(iodev, K_OBJ_RTIO_IODEV));
|
||||
Z_OOPS(Z_SYSCALL_OBJ(sensor, K_OBJ_DRIVER_SENSOR));
|
||||
Z_OOPS(Z_SYSCALL_MEMORY_READ(channels, sizeof(enum sensor_channel) * num_channels));
|
||||
return z_impl_sensor_reconfigure_read_iodev(iodev, sensor, channels, num_channels);
|
||||
}
|
||||
#include <syscalls/sensor_reconfigure_read_iodev_mrsh.c>
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -19,10 +19,14 @@
|
|||
* @{
|
||||
*/
|
||||
|
||||
#include <zephyr/types.h>
|
||||
#include <zephyr/device.h>
|
||||
#include <zephyr/sys/iterable_sections.h>
|
||||
#include <errno.h>
|
||||
#include <stdlib.h>
|
||||
|
||||
#include <zephyr/device.h>
|
||||
#include <zephyr/dsp/types.h>
|
||||
#include <zephyr/rtio/rtio.h>
|
||||
#include <zephyr/sys/iterable_sections.h>
|
||||
#include <zephyr/types.h>
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
|
|
@ -395,12 +399,170 @@ 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_ticks`.
|
||||
* @return 0 on success
|
||||
* @return <0 on error
|
||||
*/
|
||||
int (*get_timestamp)(const uint8_t *buffer, uint64_t *timestamp_ns);
|
||||
|
||||
/**
|
||||
* @brief Get the shift count of a particular channel (multiplier)
|
||||
*
|
||||
* This value can be used by shifting the q31_t value resulting in the SI unit of the
|
||||
* reading. It is guaranteed that the shift 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] shift The bit shift 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_shift)(const uint8_t *buffer, enum sensor_channel channel_type, int8_t *shift);
|
||||
|
||||
/**
|
||||
* @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_read_config {
|
||||
const struct device *sensor;
|
||||
enum sensor_channel *const channels;
|
||||
size_t count;
|
||||
const size_t max;
|
||||
};
|
||||
|
||||
/**
|
||||
* @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_READ_IODEV(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_READ_IODEV(name, dt_node, ...) \
|
||||
static enum sensor_channel __channel_array_##name[] = {__VA_ARGS__}; \
|
||||
static struct sensor_read_config __sensor_read_config_##name = { \
|
||||
.sensor = DEVICE_DT_GET(dt_node), \
|
||||
.channels = __channel_array_##name, \
|
||||
.count = ARRAY_SIZE(__channel_array_##name), \
|
||||
.max = ARRAY_SIZE(__channel_array_##name), \
|
||||
}; \
|
||||
RTIO_IODEV_DEFINE(name, &__sensor_iodev_api, &__sensor_read_config_##name)
|
||||
|
||||
/* Used to submit an RTIO sqe to the sensor's iodev */
|
||||
typedef int (*sensor_submit_t)(const struct device *sensor, struct rtio_iodev_sqe *sqe);
|
||||
|
||||
/* The default decoder API */
|
||||
extern const struct sensor_decoder_api __sensor_default_decoder;
|
||||
|
||||
/* The default sensor iodev API */
|
||||
extern const struct rtio_iodev_api __sensor_iodev_api;
|
||||
|
||||
__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;
|
||||
};
|
||||
|
||||
/**
|
||||
|
|
@ -599,6 +761,168 @@ static inline int z_impl_sensor_channel_get(const struct device *dev,
|
|||
return api->channel_get(dev, chan, val);
|
||||
}
|
||||
|
||||
#if defined(CONFIG_SENSOR_ASYNC_API) || defined(__DOXYGEN__)
|
||||
|
||||
/*
|
||||
* Generic data structure used for encoding the sample timestamp and number of channels sampled.
|
||||
*/
|
||||
struct __attribute__((__packed__)) sensor_data_generic_header {
|
||||
/* The timestamp at which the data was collected from the sensor */
|
||||
uint64_t timestamp_ns;
|
||||
|
||||
/*
|
||||
* The number of channels present in the frame. This will be the true number of elements in
|
||||
* channel_info and in the q31 values that follow the header.
|
||||
*/
|
||||
size_t num_channels;
|
||||
|
||||
/* Shift value for all samples in the frame */
|
||||
int8_t shift;
|
||||
|
||||
/* Channels present in the frame */
|
||||
enum sensor_channel channels[0];
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief checks if a given channel is a 3-axis channel
|
||||
*
|
||||
* @param[in] chan The channel to check
|
||||
* @return true if @p chan is :c:enum:`SENSOR_CHAN_ACCEL_XYZ`
|
||||
* @return true if @p chan is :c:enum:`SENSOR_CHAN_GYRO_XYZ`
|
||||
* @return true if @p chan is :c:enum:`SENSOR_CHAN_MAGN_XYZ`
|
||||
* @return false otherwise
|
||||
*/
|
||||
#define SENSOR_CHANNEL_3_AXIS(chan) \
|
||||
((chan) == SENSOR_CHAN_ACCEL_XYZ || (chan) == SENSOR_CHAN_GYRO_XYZ || \
|
||||
(chan) == SENSOR_CHAN_MAGN_XYZ)
|
||||
|
||||
/**
|
||||
* @brief Get the sensor's decoder API
|
||||
*
|
||||
* @param[in] dev The sensor device
|
||||
* @param[in] decoder Pointer to the decoder which will be set upon success
|
||||
* @return 0 on success
|
||||
* @return < 0 on error
|
||||
*/
|
||||
__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 = (const struct sensor_driver_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);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Reconfigure a reading iodev
|
||||
*
|
||||
* Allows a reconfiguration of the iodev associated with reading a sample from a sensor.
|
||||
*
|
||||
* <b>Important</b>: If the iodev is currently servicing a read operation, the data will likely be
|
||||
* invalid. Please be sure the flush or wait for all pending operations to complete before using the
|
||||
* data after a configuration change.
|
||||
*
|
||||
* It is also important that the `data` field of the iodev is a :c:struct:`sensor_read_config`.
|
||||
*
|
||||
* @param[in] iodev The iodev to reconfigure
|
||||
* @param[in] sensor The sensor to read from
|
||||
* @param[in] channels One or more channels to read
|
||||
* @param[in] num_channels The number of channels in @p channels
|
||||
* @return 0 on success
|
||||
* @return < 0 on error
|
||||
*/
|
||||
__syscall int sensor_reconfigure_read_iodev(struct rtio_iodev *iodev, const struct device *sensor,
|
||||
const enum sensor_channel *channels,
|
||||
size_t num_channels);
|
||||
|
||||
static inline int z_impl_sensor_reconfigure_read_iodev(struct rtio_iodev *iodev,
|
||||
const struct device *sensor,
|
||||
const enum sensor_channel *channels,
|
||||
size_t num_channels)
|
||||
{
|
||||
struct sensor_read_config *cfg = (struct sensor_read_config *)iodev->data;
|
||||
|
||||
if (cfg->max < num_channels) {
|
||||
return -ENOMEM;
|
||||
}
|
||||
|
||||
cfg->sensor = sensor;
|
||||
memcpy(cfg->channels, channels, num_channels * sizeof(enum sensor_channel));
|
||||
cfg->count = num_channels;
|
||||
return 0;
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Read data from a sensor.
|
||||
*
|
||||
* Using @p cfg, read one snapshot of data from the device by using the provided RTIO context
|
||||
* @p ctx. This call will generate a :c:struct:`rtio_sqe` that will leverage the RTIO's internal
|
||||
* mempool when the time comes to service the read.
|
||||
*
|
||||
* @param[in] iodev The iodev created by :c:macro:`SENSOR_DT_READ_IODEV`
|
||||
* @param[in] ctx The RTIO context to service the read
|
||||
* @param[in] userdata Optional userdata that will be available when the read is complete
|
||||
* @return 0 on success
|
||||
* @return < 0 on error
|
||||
*/
|
||||
static inline int sensor_read(struct rtio_iodev *iodev, struct rtio *ctx, void *userdata)
|
||||
{
|
||||
if (IS_ENABLED(CONFIG_USERSPACE)) {
|
||||
struct rtio_sqe sqe;
|
||||
|
||||
rtio_sqe_prep_read_with_pool(&sqe, iodev, RTIO_PRIO_NORM, userdata);
|
||||
rtio_sqe_copy_in(ctx, &sqe, 1);
|
||||
} else {
|
||||
struct rtio_sqe *sqe = rtio_sqe_acquire(ctx);
|
||||
|
||||
if (sqe == NULL) {
|
||||
return -ENOMEM;
|
||||
}
|
||||
rtio_sqe_prep_read_with_pool(sqe, iodev, RTIO_PRIO_NORM, userdata);
|
||||
}
|
||||
rtio_submit(ctx, 0);
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* @typedef sensor_processing_callback_t
|
||||
* @brief Callback function used with the helper processing function.
|
||||
*
|
||||
* @see sensor_processing_with_callback
|
||||
*
|
||||
* @param[in] result The result code of the read (0 being success)
|
||||
* @param[in] buf The data buffer holding the sensor data
|
||||
* @param[in] buf_len The length (in bytes) of the @p buf
|
||||
* @param[in] userdata The optional userdata passed to :c:func:`sensor_read`
|
||||
*/
|
||||
typedef void (*sensor_processing_callback_t)(int result, uint8_t *buf, uint32_t buf_len,
|
||||
void *userdata);
|
||||
|
||||
/**
|
||||
* @brief Helper function for common processing of sensor data.
|
||||
*
|
||||
* This function can be called in a blocking manner after :c:func:`sensor_read` or in a standalone
|
||||
* thread dedicated to processing. It will wait for a cqe from the RTIO context, once received, it
|
||||
* will decode the userdata and call the @p cb. Once the @p cb returns, the buffer will be released
|
||||
* back into @p ctx's mempool if available.
|
||||
*
|
||||
* @param[in] ctx The RTIO context to wait on
|
||||
* @param[in] cb Callback to call when data is ready for processing
|
||||
*/
|
||||
void sensor_processing_with_callback(struct rtio *ctx, sensor_processing_callback_t cb);
|
||||
|
||||
#endif /* defined(CONFIG_SENSOR_ASYNC_API) || defined(__DOXYGEN__) */
|
||||
|
||||
/**
|
||||
* @brief The value of gravitational constant in micro m/s^2.
|
||||
*/
|
||||
|
|
@ -907,6 +1231,57 @@ static inline int64_t sensor_value_to_micro(struct sensor_value *val)
|
|||
* @}
|
||||
*/
|
||||
|
||||
#if defined(CONFIG_HAS_DTS) || defined(__DOXYGEN__)
|
||||
|
||||
/**
|
||||
* @brief Get the decoder name for the current driver
|
||||
*
|
||||
* This function depends on `DT_DRV_COMPAT` being defined.
|
||||
*/
|
||||
#define SENSOR_DECODER_NAME() UTIL_CAT(DT_DRV_COMPAT, __decoder_api)
|
||||
|
||||
/**
|
||||
* @brief Statically get the decoder for a given node
|
||||
*
|
||||
* @code{.c}
|
||||
* static const sensor_decoder_api *decoder = SENSOR_DECODER_DT_GET(DT_ALIAS(accel));
|
||||
* @endcode
|
||||
*/
|
||||
#define SENSOR_DECODER_DT_GET(node_id) \
|
||||
&UTIL_CAT(DT_STRING_TOKEN_BY_IDX(node_id, compatible, 0), __decoder_api)
|
||||
|
||||
/**
|
||||
* @brief Define a decoder API
|
||||
*
|
||||
* This macro should be created once per compatible string of a sensor and will create a statically
|
||||
* referenceable decoder API.
|
||||
*
|
||||
* @code{.c}
|
||||
* SENSOR_DECODER_API_DT_DEFINE() = {
|
||||
* .get_frame_count = my_driver_get_frame_count,
|
||||
* .get_timestamp = my_driver_get_timestamp,
|
||||
* .get_shift = my_driver_get_shift,
|
||||
* .decode = my_driver_decode,
|
||||
* };
|
||||
* @endcode
|
||||
*/
|
||||
#define SENSOR_DECODER_API_DT_DEFINE() \
|
||||
COND_CODE_1(DT_HAS_COMPAT_STATUS_OKAY(DT_DRV_COMPAT), (), (static)) \
|
||||
const STRUCT_SECTION_ITERABLE(sensor_decoder_api, SENSOR_DECODER_NAME())
|
||||
|
||||
#define Z_MAYBE_SENSOR_DECODER_DECLARE_INTERNAL_IDX(node_id, prop, idx) \
|
||||
extern const struct sensor_decoder_api UTIL_CAT( \
|
||||
DT_STRING_TOKEN_BY_IDX(node_id, prop, idx), __decoder_api);
|
||||
|
||||
#define Z_MAYBE_SENSOR_DECODER_DECLARE_INTERNAL(node_id) \
|
||||
COND_CODE_1(DT_NODE_HAS_PROP(node_id, compatible), \
|
||||
(DT_FOREACH_PROP_ELEM(node_id, compatible, \
|
||||
Z_MAYBE_SENSOR_DECODER_DECLARE_INTERNAL_IDX)), \
|
||||
())
|
||||
|
||||
DT_FOREACH_STATUS_OKAY_NODE(Z_MAYBE_SENSOR_DECODER_DECLARE_INTERNAL)
|
||||
#endif /* defined(CONFIG_HAS_DTS) || defined(__DOXYGEN__) */
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -14,6 +14,10 @@
|
|||
ITERABLE_SECTION_ROM(sensor_info, 4)
|
||||
#endif
|
||||
|
||||
#if defined(CONFIG_SENSOR)
|
||||
ITERABLE_SECTION_ROM(sensor_decoder_api, 4)
|
||||
#endif
|
||||
|
||||
#if defined(CONFIG_MCUMGR)
|
||||
ITERABLE_SECTION_ROM(mcumgr_handler, 4)
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -113,7 +113,8 @@ kobjects = OrderedDict([
|
|||
("ztest_unit_test", ("CONFIG_ZTEST_NEW_API", True, False)),
|
||||
("ztest_test_rule", ("CONFIG_ZTEST_NEW_API", True, False)),
|
||||
("rtio", ("CONFIG_RTIO", False, False)),
|
||||
("rtio_iodev", ("CONFIG_RTIO", False, False))
|
||||
("rtio_iodev", ("CONFIG_RTIO", False, False)),
|
||||
("sensor_decoder_api", ("CONFIG_SENSOR", True, False))
|
||||
])
|
||||
|
||||
def kobject_to_enum(kobj):
|
||||
|
|
|
|||
Loading…
Reference in a new issue