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