1 /*
2 * Copyright 2025 CogniPilot Foundation
3 * SPDX-License-Identifier: Apache-2.0
4 */
5
6 #include <stdio.h>
7 #include <math.h>
8 #include <zephyr/device.h>
9 #include <zephyr/kernel.h>
10 #include <zephyr/ztest.h>
11 #include <zephyr/rtio/rtio.h>
12 #include <zephyr/drivers/sensor.h>
13 #include <zephyr/logging/log.h>
14
15 #define BATCH_DURATION 50
16
q31_to_double(int32_t q31_value,int8_t shift)17 double q31_to_double(int32_t q31_value, int8_t shift)
18 {
19 return ((double)q31_value) / (double)(1 << (31 - shift));
20 }
21
22 static struct rtio_sqe *streaming_handle;
23
24 static struct sensor_stream_trigger stream_trigger = {
25 .opt = SENSOR_STREAM_DATA_INCLUDE,
26 .trigger = SENSOR_TRIG_FIFO_WATERMARK,
27 };
28
29 static struct sensor_read_config stream_config = {
30 .sensor = DEVICE_DT_GET(DT_NODELABEL(icm42688_1)),
31 .is_streaming = true,
32 .triggers = &stream_trigger,
33 .count = 0,
34 .max = 1,
35 };
36
37 static RTIO_IODEV_DEFINE(iodev_stream, &__sensor_iodev_api, &stream_config);
38 RTIO_DEFINE_WITH_MEMPOOL(rtio, 4, 4, 32, 64, 4);
39
callback(int result,uint8_t * buf,uint32_t buf_len,void * userdata)40 static void callback(int result, uint8_t *buf, uint32_t buf_len, void *userdata)
41 {
42 static uint8_t decoded_buffer[127];
43
44 zassert_not_null(stream_config.sensor->api);
45 const struct sensor_decoder_api *decoder;
46
47 zassert_ok(sensor_get_decoder(stream_config.sensor, &decoder));
48 int channels[] = {
49 SENSOR_CHAN_GYRO_XYZ,
50 SENSOR_CHAN_ACCEL_XYZ,
51 };
52
53 for (int i = 0; i < ARRAY_SIZE(channels); i++) {
54 int chan = channels[i];
55 struct sensor_chan_spec ch = {
56 .chan_idx = 0,
57 .chan_type = chan,
58 };
59
60 uint32_t fit = 0;
61 int count = 0;
62
63 while (decoder->decode(buf, ch, &fit, 1, decoded_buffer) > 0) {
64 zassert(count++ < 127, "fifo overflow");
65 if (ch.chan_type == SENSOR_CHAN_ACCEL_XYZ) {
66 struct sensor_three_axis_data *data =
67 (struct sensor_three_axis_data *)decoded_buffer;
68 double x = q31_to_double(data->readings[0].values[0], data->shift);
69 double y = q31_to_double(data->readings[0].values[1], data->shift);
70 double z = q31_to_double(data->readings[0].values[2], data->shift);
71
72 zassert(fabs(x) < 1.0, "fail", "accel x out of range: %10.4f", x);
73 zassert(fabs(y) < 1.0, "fail", "accel y out of range: %10.4f", y);
74 zassert(fabs(z - 9.8) < 1.0, "fail",
75 "accel z out of range: %10.4f", z);
76 } else if (ch.chan_type == SENSOR_CHAN_GYRO_XYZ) {
77 struct sensor_three_axis_data *data =
78 (struct sensor_three_axis_data *)decoded_buffer;
79 double x = q31_to_double(data->readings[0].values[0], data->shift);
80 double y = q31_to_double(data->readings[0].values[1], data->shift);
81 double z = q31_to_double(data->readings[0].values[2], data->shift);
82
83 zassert(fabs(x) < 0.1, "fail", "gyro x out of range: %10.4f", x);
84 zassert(fabs(y) < 0.1, "fail", "gyro y out of range: %10.4f", y);
85 zassert(fabs(z) < 0.1, "fail", "gyro z out of range: %10.4f", z);
86 }
87 }
88 }
89 }
90
ZTEST(icm42688_stream,test_icm42688_stream)91 ZTEST(icm42688_stream, test_icm42688_stream)
92 {
93 stream_config.count = 1;
94 struct sensor_value val_in = { BATCH_DURATION, 0 };
95
96 zassert_ok(sensor_attr_set(
97 stream_config.sensor,
98 SENSOR_CHAN_ALL,
99 SENSOR_ATTR_BATCH_DURATION, &val_in));
100 struct sensor_value val_out;
101
102 zassert_ok(sensor_attr_get(
103 stream_config.sensor,
104 SENSOR_CHAN_ALL,
105 SENSOR_ATTR_BATCH_DURATION, &val_out));
106 zassert_equal(val_in.val1, val_out.val1);
107 zassert_ok(sensor_stream(&iodev_stream, &rtio, NULL,
108 &streaming_handle));
109 sensor_processing_with_callback(&rtio, callback);
110 zassert_ok(rtio_sqe_cancel(streaming_handle));
111 }
112
icm42688_stream_setup(void)113 static void *icm42688_stream_setup(void)
114 {
115 return NULL;
116 }
117
118
119 ZTEST_SUITE(icm42688_stream, NULL, icm42688_stream_setup, NULL, NULL, NULL);
120
