From 5ca25020d56d7fb606720ba2c7bdbbcdbfb24dc2 Mon Sep 17 00:00:00 2001 From: Yuval Peress Date: Wed, 26 Apr 2023 01:13:33 -0600 Subject: [PATCH] DNM: hacky example using the previously added APIs --- samples/sensor/sensor_shell/prj.conf | 11 +- samples/sensor/sensor_shell/src/main.c | 188 +++++++++++++++++++++++++ 2 files changed, 198 insertions(+), 1 deletion(-) diff --git a/samples/sensor/sensor_shell/prj.conf b/samples/sensor/sensor_shell/prj.conf index 2a9e6a342216986..add631721119e04 100644 --- a/samples/sensor/sensor_shell/prj.conf +++ b/samples/sensor/sensor_shell/prj.conf @@ -4,4 +4,13 @@ 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_RTIO_CONSUME_SEM=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 235cc58982cd4fd..12be997797837ec 100644 --- a/samples/sensor/sensor_shell/src/main.c +++ b/samples/sensor/sensor_shell/src/main.c @@ -6,11 +6,93 @@ #include #include +#include #include #include +#include +#include + +#define LOOP_DURATION_MS 500 +#define BUSY_WAIT_DURATION_US 0 +#define DEDICATED_LOOP_THREAD 0 + LOG_MODULE_REGISTER(app); +static const struct device *accel0_dev = DEVICE_DT_GET(DT_ALIAS(accel0)); + +SENSOR_DT_READ_CONFIG(accel0, DT_ALIAS(accel0), SENSOR_CHAN_ACCEL_XYZ, SENSOR_CHAN_GYRO_XYZ); +SENSOR_DT_READ_CONFIG(magn0, DT_ALIAS(magn0), SENSOR_CHAN_MAGN_XYZ); + +RTIO_EXECUTOR_SIMPLE_DEFINE(sensor_executor); +RTIO_DEFINE_WITH_MEMPOOL(rtio_ctx, &sensor_executor, 1, 1, 2, 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; + } + 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++; + } +} + +#if DEDICATED_LOOP_THREAD +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, + K_PRIO_COOP(10), 0, 0); +#endif /* DEDICATED_LOOP_THREAD */ + enum sample_stats_state { SAMPLE_STATS_STATE_UNINITIALIZED = 0, SAMPLE_STATS_STATE_ENABLED, @@ -24,6 +106,7 @@ struct sample_stats { enum sample_stats_state state; }; +#if 0 static void data_ready_trigger_handler(const struct device *sensor, const struct sensor_trigger *trigger) { @@ -73,9 +156,11 @@ static void data_ready_trigger_handler(const struct device *sensor, } } } +#endif int main(void) { +#if 0 STRUCT_SECTION_FOREACH(sensor_info, sensor) { struct sensor_trigger trigger = { @@ -84,5 +169,108 @@ int main(void) }; sensor_trigger_set(sensor->dev, &trigger, data_ready_trigger_handler); } +#endif + + 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 start_ts = k_uptime_get(); + uint64_t end_ts = start_ts; + + 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); + } + z_sensor_processing_loop(&rtio_ctx, sensor_processing_callback); + } 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_samples) / (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); + printk("Completion overrun count %d\n", (uint32_t)atomic_get(&rtio_ctx.xcqcnt)); + + num_requests = 0; + failed_reads = 0; + num_samples = 0; + + start_ts = k_uptime_get(); + end_ts = start_ts; + do { + int rc = sensor_sample_fetch(accel0_dev); + + num_requests++; + 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("\n\n*************************\n"); + 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_samples) / (duration * 1000)); + printk("Num Requests: %d\n", num_requests); + printk("Num Samples: %d\n", num_samples); + return 0; }