diff --git a/drivers/sensor/sensor_shell.c b/drivers/sensor/sensor_shell.c index 7366ec816c93769..37639a7b6c20ca0 100644 --- a/drivers/sensor/sensor_shell.c +++ b/drivers/sensor/sensor_shell.c @@ -4,12 +4,14 @@ * SPDX-License-Identifier: Apache-2.0 */ -#include +#include #include #include -#include + #include #include +#include +#include #define SENSOR_GET_HELP \ "Get sensor data. Channel names are optional. All channels are read " \ @@ -113,6 +115,19 @@ enum dynamic_command_context { static enum dynamic_command_context current_cmd_ctx = NONE; +/* Crate a single common config for one-shot reading */ +static enum sensor_channel iodev_sensor_shell_channels[SENSOR_CHAN_ALL]; +static struct sensor_read_config iodev_sensor_shell_read_config = { + .sensor = NULL, + .channels = iodev_sensor_shell_channels, + .count = 0, + .max = ARRAY_SIZE(iodev_sensor_shell_channels), +}; +RTIO_IODEV_DEFINE(iodev_sensor_shell_read, &__sensor_iodev_api, &iodev_sensor_shell_read_config); + +/* Create the RTIO context to service the reading */ +RTIO_DEFINE_WITH_MEMPOOL(sensor_read_rtio, 8, 8, 32, 64, 4); + static int parse_named_int(const char *name, const char *heystack[], size_t count) { char *endptr; @@ -175,43 +190,71 @@ static int parse_sensor_value(const char *val_str, struct sensor_value *out) return 0; } -static int handle_channel_by_name(const struct shell *shell_ptr, const struct device *dev, - const char *channel_name) +struct sensor_shell_processing_context { + const struct device *dev; + const struct shell *sh; +}; + +static void sensor_shell_processing_callback(int result, uint8_t *buf, uint32_t buf_len, + void *userdata) { - struct sensor_value value[3]; - int err; - const int i = - parse_named_int(channel_name, sensor_channel_name, ARRAY_SIZE(sensor_channel_name)); + struct sensor_shell_processing_context *ctx = userdata; + const struct sensor_decoder_api *decoder; + sensor_frame_iterator_t fit = {0}; + sensor_channel_iterator_t cit = {0}; + uint64_t timestamp; + enum sensor_channel channel; + q31_t q; + int rc; - if (i < 0) { - shell_error(shell_ptr, "Channel not supported (%s)", channel_name); - return i; + ARG_UNUSED(buf_len); + + if (result < 0) { + shell_error(ctx->sh, "Read failed"); + return; } - err = sensor_channel_get(dev, i, value); - if (err < 0) { - return err; + rc = sensor_get_decoder(ctx->dev, &decoder); + if (rc != 0) { + shell_error(ctx->sh, "Failed to get decoder for '%s'", ctx->dev->name); + return; } - if (i >= ARRAY_SIZE(sensor_channel_name)) { - shell_print(shell_ptr, "channel idx=%d value = %10.6f", i, - sensor_value_to_double(&value[0])); - } else if (i != SENSOR_CHAN_ACCEL_XYZ && i != SENSOR_CHAN_GYRO_XYZ && - i != SENSOR_CHAN_MAGN_XYZ) { - shell_print(shell_ptr, "channel idx=%d %s = %10.6f", i, sensor_channel_name[i], - sensor_value_to_double(&value[0])); - } else { - /* clang-format off */ - shell_print(shell_ptr, - "channel idx=%d %s x = %10.6f y = %10.6f z = %10.6f", - i, sensor_channel_name[i], - sensor_value_to_double(&value[0]), - sensor_value_to_double(&value[1]), - sensor_value_to_double(&value[2])); - /* clang-format on */ + rc = decoder->get_timestamp(buf, ×tamp); + if (rc != 0) { + shell_error(ctx->sh, "Failed to get fetch timestamp for '%s'", ctx->dev->name); + return; } + shell_print(ctx->sh, "Got samples at %" PRIu64 " ns", timestamp); - return 0; + while (decoder->decode(buf, &fit, &cit, &channel, &q, 1) > 0) { + int8_t shift; + + rc = decoder->get_shift(buf, channel, &shift); + if (rc != 0) { + shell_error(ctx->sh, "Failed to get bitshift for channel %d", channel); + continue; + } + + int64_t scaled_value = (int64_t)q << shift; + bool is_negative = scaled_value < 0; + int numerator; + int denominator; + + scaled_value = llabs(scaled_value); + numerator = (int)FIELD_GET(GENMASK64(31 + shift, 31), scaled_value); + denominator = + (int)((FIELD_GET(GENMASK64(30, 0), scaled_value) * 1000000) / INT32_MAX); + + if (channel >= ARRAY_SIZE(sensor_channel_name)) { + shell_print(ctx->sh, "channel idx=%d value=%s%d.%06d", channel, + is_negative ? "-" : "", numerator, denominator); + } else { + shell_print(ctx->sh, "channel idx=%d %s value=%s%d.%06d", channel, + sensor_channel_name[channel], is_negative ? "-" : "", numerator, + denominator); + } + } } static int cmd_get_sensor(const struct shell *sh, size_t argc, char *argv[]) @@ -225,27 +268,50 @@ static int cmd_get_sensor(const struct shell *sh, size_t argc, char *argv[]) return -ENODEV; } - err = sensor_sample_fetch(dev); - if (err < 0) { - shell_error(sh, "Failed to read sensor: %d", err); - } - if (argc == 2) { /* read all channels */ - for (int i = 0; i < ARRAY_SIZE(sensor_channel_name); i++) { - if (sensor_channel_name[i]) { - handle_channel_by_name(sh, dev, sensor_channel_name[i]); + int count = 0; + + for (int i = 0; i < ARRAY_SIZE(iodev_sensor_shell_channels); ++i) { + if (SENSOR_CHANNEL_3_AXIS(i)) { + continue; } + iodev_sensor_shell_channels[count++] = i; } + iodev_sensor_shell_read_config.count = count; } else { - for (int i = 2; i < argc; i++) { - err = handle_channel_by_name(sh, dev, argv[i]); - if (err < 0) { + /* read specific channels */ + iodev_sensor_shell_read_config.count = 0; + for (int i = 2; i < argc; ++i) { + int chan = parse_named_int(argv[i], sensor_channel_name, + ARRAY_SIZE(sensor_channel_name)); + + if (chan < 0) { shell_error(sh, "Failed to read channel (%s)", argv[i]); + continue; } + iodev_sensor_shell_channels[iodev_sensor_shell_read_config.count++] = + chan; } } + /* TODO(yperess) use sensor_reconfigure_read_iodev? */ + if (iodev_sensor_shell_read_config.count == 0) { + shell_error(sh, "No channels to read, bailing"); + return -EINVAL; + } + iodev_sensor_shell_read_config.sensor = dev; + + struct sensor_shell_processing_context ctx = { + .dev = dev, + .sh = sh, + }; + err = sensor_read(&iodev_sensor_shell_read, &sensor_read_rtio, &ctx); + if (err < 0) { + shell_error(sh, "Failed to read sensor: %d", err); + } + z_sensor_processing_loop(&sensor_read_rtio, sensor_shell_processing_callback); + return 0; } diff --git a/samples/sensor/sensor_shell/CMakeLists.txt b/samples/sensor/sensor_shell/CMakeLists.txt index 86a0a84923dda11..b33730c91ff63a4 100644 --- a/samples/sensor/sensor_shell/CMakeLists.txt +++ b/samples/sensor/sensor_shell/CMakeLists.txt @@ -5,5 +5,7 @@ cmake_minimum_required(VERSION 3.20.0) find_package(Zephyr REQUIRED HINTS $ENV{ZEPHYR_BASE}) project(sensor_shell) -FILE(GLOB app_sources src/*.c) -target_sources(app PRIVATE ${app_sources}) +target_sources(app PRIVATE src/main.c) +target_sources_ifdef(CONFIG_INIT_TRIG_DATA_READY app PRIVATE src/trigger.c) + +target_include_directories(app PRIVATE include) diff --git a/samples/sensor/sensor_shell/Kconfig b/samples/sensor/sensor_shell/Kconfig index db47d9fa0bc24f4..370c79dac208eae 100644 --- a/samples/sensor/sensor_shell/Kconfig +++ b/samples/sensor/sensor_shell/Kconfig @@ -9,3 +9,9 @@ config SAMPLE_PRINT_TIMEOUT_MS interrupt handler will collect data. source "Kconfig.zephyr" + +config INIT_TRIG_DATA_READY + bool "Register data ready triggers for all sensors on start" + help + When the application starts, automatically register data ready trigger + listeners to all available sensors. diff --git a/samples/sensor/sensor_shell/include/trigger.h b/samples/sensor/sensor_shell/include/trigger.h new file mode 100644 index 000000000000000..eb5a35ffcfa11ec --- /dev/null +++ b/samples/sensor/sensor_shell/include/trigger.h @@ -0,0 +1,16 @@ +/* + * Copyright (c) 2023 Google LLC + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#ifndef ZEPHYR_SAMPLES_SENSOR_SENSOR_SHELL_INCLUDE_TRIGGER_H +#define ZEPHYR_SAMPLES_SENSOR_SENSOR_SHELL_INCLUDE_TRIGGER_H + +#include +#include + +void sensor_shell_data_ready_trigger_handler(const struct device *sensor, + const struct sensor_trigger *trigger); + +#endif /* ZEPHYR_SAMPLES_SENSOR_SENSOR_SHELL_INCLUDE_TRIGGER_H */ diff --git a/samples/sensor/sensor_shell/prj.conf b/samples/sensor/sensor_shell/prj.conf index 2a9e6a342216986..d3189e39aeac769 100644 --- a/samples/sensor/sensor_shell/prj.conf +++ b/samples/sensor/sensor_shell/prj.conf @@ -5,3 +5,4 @@ CONFIG_SENSOR_SHELL=y CONFIG_SENSOR_INFO=y CONFIG_LOG=y +CONFIG_RTIO_CONSUME_SEM=y diff --git a/samples/sensor/sensor_shell/src/main.c b/samples/sensor/sensor_shell/src/main.c index 235cc58982cd4fd..49b4a66be6a825e 100644 --- a/samples/sensor/sensor_shell/src/main.c +++ b/samples/sensor/sensor_shell/src/main.c @@ -9,80 +9,22 @@ #include #include -LOG_MODULE_REGISTER(app); - -enum sample_stats_state { - SAMPLE_STATS_STATE_UNINITIALIZED = 0, - SAMPLE_STATS_STATE_ENABLED, - SAMPLE_STATS_STATE_DISABLED, -}; - -struct sample_stats { - int64_t accumulator; - uint32_t count; - uint64_t sample_window_start; - enum sample_stats_state state; -}; - -static void data_ready_trigger_handler(const struct device *sensor, - const struct sensor_trigger *trigger) -{ - static struct sample_stats stats[SENSOR_CHAN_ALL]; - const int64_t now = k_uptime_get(); - struct sensor_value value; - - if (sensor_sample_fetch(sensor)) { - LOG_ERR("Failed to fetch samples on data ready handler"); - } - for (int i = 0; i < SENSOR_CHAN_ALL; ++i) { - int rc; - - /* Skip disabled channels */ - if (stats[i].state == SAMPLE_STATS_STATE_DISABLED) { - continue; - } - /* Skip 3 axis channels */ - if (i == SENSOR_CHAN_ACCEL_XYZ || i == SENSOR_CHAN_GYRO_XYZ || - i == SENSOR_CHAN_MAGN_XYZ) { - continue; - } +#include "trigger.h" - rc = sensor_channel_get(sensor, i, &value); - if (rc == -ENOTSUP && stats[i].state == SAMPLE_STATS_STATE_UNINITIALIZED) { - /* Stop reading this channel if the driver told us it's not supported. */ - stats[i].state = SAMPLE_STATS_STATE_DISABLED; - } - if (rc != 0) { - /* Skip on any error. */ - continue; - } - /* Do something with the data */ - stats[i].accumulator += value.val1 * INT64_C(1000000) + value.val2; - if (stats[i].count++ == 0) { - stats[i].sample_window_start = now; - } else if (now > stats[i].sample_window_start + CONFIG_SAMPLE_PRINT_TIMEOUT_MS) { - int64_t micro_value = stats[i].accumulator / stats[i].count; - - value.val1 = micro_value / 1000000; - value.val2 = (int32_t)llabs(micro_value - (value.val1 * 1000000)); - LOG_INF("chan=%d, num_samples=%u, data=%d.%06d", i, stats[i].count, - value.val1, value.val2); - - stats[i].accumulator = 0; - stats[i].count = 0; - } - } -} +LOG_MODULE_REGISTER(app); 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); + if (IS_ENABLED(CONFIG_INIT_TRIG_DATA_READY)) { + 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, + sensor_shell_data_ready_trigger_handler); + } } return 0; } diff --git a/samples/sensor/sensor_shell/src/trigger.c b/samples/sensor/sensor_shell/src/trigger.c new file mode 100644 index 000000000000000..ee9272313d9acd8 --- /dev/null +++ b/samples/sensor/sensor_shell/src/trigger.c @@ -0,0 +1,78 @@ +/* + * Copyright (c) 2023 Google LLC + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include "trigger.h" + +#include +#include +#include + +LOG_MODULE_DECLARE(app); + +enum sample_stats_state { + SAMPLE_STATS_STATE_UNINITIALIZED = 0, + SAMPLE_STATS_STATE_ENABLED, + SAMPLE_STATS_STATE_DISABLED, +}; + +struct sample_stats { + int64_t accumulator; + uint32_t count; + uint64_t sample_window_start; + enum sample_stats_state state; +}; + +void sensor_shell_data_ready_trigger_handler(const struct device *sensor, + const struct sensor_trigger *trigger) +{ + static struct sample_stats stats[SENSOR_CHAN_ALL]; + const int64_t now = k_uptime_get(); + struct sensor_value value; + + ARG_UNUSED(trigger); + + if (sensor_sample_fetch(sensor)) { + LOG_ERR("Failed to fetch samples on data ready handler"); + } + for (int i = 0; i < SENSOR_CHAN_ALL; ++i) { + int rc; + + /* Skip disabled channels */ + if (stats[i].state == SAMPLE_STATS_STATE_DISABLED) { + continue; + } + /* Skip 3 axis channels */ + if (i == SENSOR_CHAN_ACCEL_XYZ || i == SENSOR_CHAN_GYRO_XYZ || + i == SENSOR_CHAN_MAGN_XYZ) { + continue; + } + + rc = sensor_channel_get(sensor, i, &value); + if (rc == -ENOTSUP && stats[i].state == SAMPLE_STATS_STATE_UNINITIALIZED) { + /* Stop reading this channel if the driver told us it's not supported. */ + stats[i].state = SAMPLE_STATS_STATE_DISABLED; + } + if (rc != 0) { + /* Skip on any error. */ + continue; + } + /* Do something with the data */ + stats[i].accumulator += value.val1 * INT64_C(1000000) + value.val2; + if (stats[i].count++ == 0) { + stats[i].sample_window_start = now; + } else if (now > stats[i].sample_window_start + CONFIG_SAMPLE_PRINT_TIMEOUT_MS) { + int64_t micro_value = stats[i].accumulator / stats[i].count; + + value.val1 = micro_value / 1000000; + value.val2 = (int32_t)llabs(micro_value - (value.val1 * 1000000)); + LOG_INF("chan=%d, num_samples=%u, data=%d.%06d", i, stats[i].count, + value.val1, value.val2); + + stats[i].accumulator = 0; + stats[i].count = 0; + } + } +}