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