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(stream_iodev, id)
26 #define STREAM_IODEV_PTR(id, _) &STREAM_IODEV_SYM(id)
27
28 #define STREAM_TRIGGERS \
29 { SENSOR_TRIG_FIFO_FULL, SENSOR_STREAM_DATA_NOP }, \
30 { SENSOR_TRIG_FIFO_WATERMARK, SENSOR_STREAM_DATA_INCLUDE }
31
32 #define STREAM_DEFINE_IODEV(id, _) \
33 SENSOR_DT_STREAM_IODEV( \
34 STREAM_IODEV_SYM(id), \
35 STREAMDEV_ALIAS(id), \
36 STREAM_TRIGGERS);
37
38 LISTIFY(NUM_SENSORS, STREAM_DEFINE_IODEV, (;));
39
40 struct rtio_iodev *iodevs[NUM_SENSORS] = { LISTIFY(NUM_SENSORS, STREAM_IODEV_PTR, (,)) };
41
42 RTIO_DEFINE_WITH_MEMPOOL(stream_ctx, NUM_SENSORS, NUM_SENSORS,
43 NUM_SENSORS * 20, 256, sizeof(void *));
44
45 struct sensor_chan_spec accel_chan = { SENSOR_CHAN_ACCEL_XYZ, 0 };
46 struct sensor_chan_spec gyro_chan = { SENSOR_CHAN_GYRO_XYZ, 0 };
47 struct sensor_chan_spec temp_chan = { SENSOR_CHAN_DIE_TEMP, 0 };
48 struct sensor_chan_spec rot_vector_chan = { SENSOR_CHAN_GAME_ROTATION_VECTOR, 0 };
49 struct sensor_chan_spec gravity_chan = { SENSOR_CHAN_GRAVITY_VECTOR, 0 };
50 struct sensor_chan_spec gbias_chan = { SENSOR_CHAN_GBIAS_XYZ, 0 };
51
52 #define TASK_STACK_SIZE 2048ul
53 static K_THREAD_STACK_ARRAY_DEFINE(thread_stack, NUM_SENSORS, TASK_STACK_SIZE);
54 static struct k_thread thread_id[NUM_SENSORS];
55
print_stream(void * p1,void * p2,void * p3)56 static void print_stream(void *p1, void *p2, void *p3)
57 {
58 const struct device *dev = (const struct device *)p1;
59 struct rtio_iodev *iodev = (struct rtio_iodev *)p2;
60 int rc = 0;
61 const struct sensor_decoder_api *decoder;
62 struct rtio_cqe *cqe;
63 uint8_t *buf;
64 uint32_t buf_len;
65 struct rtio_sqe *handle;
66 uint8_t accel_buf[128] = { 0 };
67 uint8_t gyro_buf[128] = { 0 };
68 uint8_t temp_buf[64] = { 0 };
69 uint8_t rot_vect_buf[128] = { 0 };
70 uint8_t gravity_buf[128] = { 0 };
71 uint8_t gbias_buf[128] = { 0 };
72 struct sensor_three_axis_data *accel_data = (struct sensor_three_axis_data *)accel_buf;
73 struct sensor_three_axis_data *gyro_data = (struct sensor_three_axis_data *)gyro_buf;
74 struct sensor_q31_data *temp_data = (struct sensor_q31_data *)temp_buf;
75 struct sensor_game_rotation_vector_data *rot_vect_data =
76 (struct sensor_game_rotation_vector_data *)rot_vect_buf;
77 struct sensor_three_axis_data *gravity_data = (struct sensor_three_axis_data *)gravity_buf;
78 struct sensor_three_axis_data *gbias_data = (struct sensor_three_axis_data *)gbias_buf;
79
80 /* Start the stream */
81 printk("sensor_stream\n");
82 sensor_stream(iodev, &stream_ctx, NULL, &handle);
83
84 while (1) {
85 cqe = rtio_cqe_consume_block(&stream_ctx);
86
87 if (cqe->result != 0) {
88 printk("async read failed %d\n", cqe->result);
89 return;
90 }
91
92 rc = rtio_cqe_get_mempool_buffer(&stream_ctx, cqe, &buf, &buf_len);
93
94 if (rc != 0) {
95 printk("get mempool buffer failed %d\n", rc);
96 return;
97 }
98
99 const struct device *sensor = dev;
100
101 rtio_cqe_release(&stream_ctx, cqe);
102
103 rc = sensor_get_decoder(sensor, &decoder);
104
105 if (rc != 0) {
106 printk("sensor_get_decoder failed %d\n", rc);
107 return;
108 }
109
110 /* Frame iterator values when data comes from a FIFO */
111 uint32_t accel_fit = 0, gyro_fit = 0;
112 uint32_t temp_fit = 0;
113 uint32_t rot_vect_fit = 0, gravity_fit = 0, gbias_fit = 0;
114
115 /* Number of sensor data frames */
116 uint16_t xl_count, gy_count, tp_count;
117 uint16_t rot_vect_count, gravity_count, gbias_count, frame_count;
118
119 rc = decoder->get_frame_count(buf, accel_chan, &xl_count);
120 rc += decoder->get_frame_count(buf, gyro_chan, &gy_count);
121 rc += decoder->get_frame_count(buf, temp_chan, &tp_count);
122 rc += decoder->get_frame_count(buf, rot_vector_chan, &rot_vect_count);
123 rc += decoder->get_frame_count(buf, gravity_chan, &gravity_count);
124 rc += decoder->get_frame_count(buf, gbias_chan, &gbias_count);
125
126 if (rc != 0) {
127 printk("sensor_get_frame failed %d\n", rc);
128 return;
129 }
130
131 frame_count = xl_count + gy_count + tp_count;
132 frame_count += rot_vect_count + gravity_count + gbias_count;
133
134 /* If a tap has occurred lets print it out */
135 if (decoder->has_trigger(buf, SENSOR_TRIG_TAP)) {
136 printk("Tap! Sensor %s\n", dev->name);
137 }
138
139 /* Decode all available sensor FIFO frames */
140 printk("FIFO count - %d\n", frame_count);
141
142 int i = 0;
143
144 while (i < frame_count) {
145 int8_t c = 0;
146
147 /* decode and print Accelerometer FIFO frames */
148 c = decoder->decode(buf, accel_chan, &accel_fit, 8, accel_data);
149
150 for (int k = 0; k < c; k++) {
151 printk("XL data for %s %lluns (%" PRIq(6) ", %" PRIq(6)
152 ", %" PRIq(6) ")\n", dev->name,
153 PRIsensor_three_axis_data_arg(*accel_data, k));
154 }
155 i += c;
156
157 /* decode and print Gyroscope FIFO frames */
158 c = decoder->decode(buf, gyro_chan, &gyro_fit, 8, gyro_data);
159
160 for (int k = 0; k < c; k++) {
161 printk("GY data for %s %lluns (%" PRIq(6) ", %" PRIq(6)
162 ", %" PRIq(6) ")\n", dev->name,
163 PRIsensor_three_axis_data_arg(*gyro_data, k));
164 }
165 i += c;
166
167 /* decode and print Temperature FIFO frames */
168 c = decoder->decode(buf, temp_chan, &temp_fit, 4, temp_data);
169
170 for (int k = 0; k < c; k++) {
171 printk("TP data for %s %lluns %s%d.%d °C\n", dev->name,
172 PRIsensor_q31_data_arg(*temp_data, k));
173 }
174 i += c;
175
176 /* decode and print Game Rotation Vector FIFO frames */
177 c = decoder->decode(buf, rot_vector_chan, &rot_vect_fit, 8, rot_vect_data);
178
179 for (int k = 0; k < c; k++) {
180 printk("ROT data for %s %lluns (%" PRIq(6) ", %" PRIq(6)
181 ", %" PRIq(6) ", %" PRIq(6) ")\n", dev->name,
182 PRIsensor_game_rotation_vector_data_arg(*rot_vect_data, k));
183 }
184 i += c;
185
186 /* decode and print Gravity Vector FIFO frames */
187 c = decoder->decode(buf, gravity_chan, &gravity_fit, 8, gravity_data);
188
189 for (int k = 0; k < c; k++) {
190 printk("GV data for %s %lluns (%" PRIq(6) ", %" PRIq(6)
191 ", %" PRIq(6) ")\n", dev->name,
192 PRIsensor_three_axis_data_arg(*gravity_data, k));
193 }
194 i += c;
195
196 /* decode and print Gyroscope GBIAS FIFO frames */
197 c = decoder->decode(buf, gbias_chan, &gbias_fit, 8, gbias_data);
198
199 for (int k = 0; k < c; k++) {
200 printk("GY GBIAS data for %s %lluns (%" PRIq(6) ", %" PRIq(6)
201 ", %" PRIq(6) ")\n", dev->name,
202 PRIsensor_three_axis_data_arg(*gbias_data, k));
203 }
204 i += c;
205 }
206
207 rtio_release_buffer(&stream_ctx, buf, buf_len);
208 }
209 }
210
check_sensor_is_off(const struct device * dev)211 static void check_sensor_is_off(const struct device *dev)
212 {
213 int ret;
214 struct sensor_value odr;
215
216 ret = sensor_attr_get(dev, SENSOR_CHAN_ACCEL_XYZ, SENSOR_ATTR_SAMPLING_FREQUENCY, &odr);
217
218 /* Check if accel is off */
219 if (ret != 0 || (odr.val1 == 0 && odr.val2 == 0)) {
220 printk("%s WRN : accelerometer device is off\n", dev->name);
221 }
222
223 ret = sensor_attr_get(dev, SENSOR_CHAN_GYRO_XYZ, SENSOR_ATTR_SAMPLING_FREQUENCY, &odr);
224
225 /* Check if gyro is off */
226 if (ret != 0 || (odr.val1 == 0 && odr.val2 == 0)) {
227 printk("%s WRN : gyroscope device is off\n", dev->name);
228 }
229 }
230
main(void)231 int main(void)
232 {
233 for (size_t i = 0; i < ARRAY_SIZE(sensors); i++) {
234 if (!device_is_ready(sensors[i])) {
235 printk("sensor: device %s not ready.\n", sensors[i]->name);
236 return 0;
237 }
238 check_sensor_is_off(sensors[i]);
239
240 k_thread_create(&thread_id[i], thread_stack[i], TASK_STACK_SIZE, print_stream,
241 (void *)sensors[i], (void *)iodevs[i], NULL, K_PRIO_COOP(5),
242 K_INHERIT_PERMS, K_FOREVER);
243 k_thread_start(&thread_id[i]);
244 }
245
246 return 0;
247 }
248
