1 /*
2 * Copyright (c) 2024 STMicroelectronics
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <stdio.h>
8 #include <stdlib.h>
9
10 #include <zephyr/device.h>
11 #include <zephyr/drivers/sensor.h>
12 #include <zephyr/sys/util_macro.h>
13 #include <zephyr/kernel.h>
14 #include <zephyr/rtio/rtio.h>
15
16 #define STREAMDEV_ALIAS(i) DT_ALIAS(_CONCAT(stream, i))
17 #define STREAMDEV_DEVICE(i, _) \
18 IF_ENABLED(DT_NODE_EXISTS(STREAMDEV_ALIAS(i)), (DEVICE_DT_GET(STREAMDEV_ALIAS(i)),))
19 #define NUM_SENSORS 1
20
21 /* support up to 10 sensors */
22 static const struct device *const sensors[] = { LISTIFY(10, STREAMDEV_DEVICE, ()) };
23
24 #define STREAM_IODEV_SYM(id) CONCAT(accel_iodev, id)
25 #define STREAM_IODEV_PTR(id, _) &STREAM_IODEV_SYM(id)
26
27 #define STREAM_TRIGGERS \
28 { SENSOR_TRIG_DATA_READY, SENSOR_STREAM_DATA_INCLUDE }
29
30 #define STREAM_DEFINE_IODEV(id, _) \
31 SENSOR_DT_STREAM_IODEV( \
32 STREAM_IODEV_SYM(id), \
33 STREAMDEV_ALIAS(id), \
34 STREAM_TRIGGERS);
35
36 LISTIFY(NUM_SENSORS, STREAM_DEFINE_IODEV, (;));
37
38 struct rtio_iodev *iodevs[NUM_SENSORS] = { LISTIFY(NUM_SENSORS, STREAM_IODEV_PTR, (,)) };
39
40 RTIO_DEFINE_WITH_MEMPOOL(stream_ctx, NUM_SENSORS, NUM_SENSORS,
41 NUM_SENSORS * 20, 256, sizeof(void *));
42
43 struct sensor_chan_spec accel_chan = { SENSOR_CHAN_ACCEL_XYZ, 0 };
44
45 static uint8_t accel_buf[128] = { 0 };
46
print_accels_stream(const struct device * dev,struct rtio_iodev * iodev)47 static int print_accels_stream(const struct device *dev, struct rtio_iodev *iodev)
48 {
49 int rc = 0;
50 const struct sensor_decoder_api *decoder;
51 struct rtio_cqe *cqe;
52 uint8_t *buf;
53 uint32_t buf_len;
54 struct rtio_sqe *handles[NUM_SENSORS];
55 struct sensor_three_axis_data *accel_data = (struct sensor_three_axis_data *)accel_buf;
56
57 /* Start the streams */
58 for (int i = 0; i < NUM_SENSORS; i++) {
59 printk("sensor_stream\n");
60 sensor_stream(iodevs[i], &stream_ctx, NULL, &handles[i]);
61 }
62
63 while (1) {
64 cqe = rtio_cqe_consume_block(&stream_ctx);
65
66 if (cqe->result != 0) {
67 printk("async read failed %d\n", cqe->result);
68 return cqe->result;
69 }
70
71 rc = rtio_cqe_get_mempool_buffer(&stream_ctx, cqe, &buf, &buf_len);
72
73 if (rc != 0) {
74 printk("get mempool buffer failed %d\n", rc);
75 return rc;
76 }
77
78 const struct device *sensor = dev;
79
80 rtio_cqe_release(&stream_ctx, cqe);
81
82 rc = sensor_get_decoder(sensor, &decoder);
83
84 if (rc != 0) {
85 printk("sensor_get_decoder failed %d\n", rc);
86 return rc;
87 }
88
89 /* Frame iterator values */
90 uint32_t accel_fit = 0;
91
92 /* Number of sensor data frames */
93 uint16_t xl_count, frame_count;
94
95 rc = decoder->get_frame_count(buf, accel_chan, &xl_count);
96
97 if (rc != 0) {
98 printk("sensor_get_frame failed %d\n", rc);
99 return rc;
100 }
101
102 frame_count = xl_count;
103
104 /* If a tap has occurred lets print it out */
105 if (decoder->has_trigger(buf, SENSOR_TRIG_TAP)) {
106 printk("Tap! Sensor %s\n", dev->name);
107 }
108
109
110 int8_t c = 0;
111
112 /* decode and print Accelerometer frames */
113 c = decoder->decode(buf, accel_chan, &accel_fit, 1, accel_data);
114
115 printk("XL data for %s %lluns (%" PRIq(6) ", %" PRIq(6)
116 ", %" PRIq(6) ")\n", dev->name,
117 PRIsensor_three_axis_data_arg(*accel_data, 0));
118
119 rtio_release_buffer(&stream_ctx, buf, buf_len);
120 }
121
122 return rc;
123 }
124
check_sensor_is_off(const struct device * dev)125 static int check_sensor_is_off(const struct device *dev)
126 {
127 int ret;
128 struct sensor_value odr;
129
130 ret = sensor_attr_get(dev, SENSOR_CHAN_ACCEL_XYZ, SENSOR_ATTR_SAMPLING_FREQUENCY, &odr);
131
132 /* Check if accel is off */
133 if (ret != 0 || (odr.val1 == 0 && odr.val2 == 0)) {
134 printk("%s WRN : accelerometer device is off\n", dev->name);
135 }
136
137 return 0;
138 }
139
main(void)140 int main(void)
141 {
142 int ret;
143
144 for (size_t i = 0; i < ARRAY_SIZE(sensors); i++) {
145 if (!device_is_ready(sensors[i])) {
146 printk("sensor: device %s not ready.\n", sensors[i]->name);
147 return 0;
148 }
149 check_sensor_is_off(sensors[i]);
150 }
151
152 while (1) {
153 for (size_t i = 0; i < ARRAY_SIZE(sensors); i++) {
154 ret = print_accels_stream(sensors[i], iodevs[i]);
155
156 if (ret < 0) {
157 return 0;
158 }
159 }
160 k_msleep(1000);
161 }
162 return 0;
163 }
164
