| /* |
| * Copyright (c) 2018 Diego Sueiro |
| * |
| * SPDX-License-Identifier: Apache-2.0 |
| */ |
| |
| #include <stdlib.h> |
| #include <zephyr/drivers/sensor.h> |
| #include <zephyr/kernel.h> |
| #include <zephyr/logging/log.h> |
| #include <zephyr/sys/iterable_sections.h> |
| |
| 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; |
| } |
| |
| 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; |
| } |
| } |
| } |
| |
| 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); |
| } |
| return 0; |
| } |