1 /*
2 * Copyright (c) 2024 STMicroelectronics
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <zephyr/kernel.h>
8 #include <zephyr/device.h>
9 #include <zephyr/drivers/sensor.h>
10 #include <stdio.h>
11 #include <zephyr/sys/util.h>
12
13 #ifdef CONFIG_LPS2XDF_TRIGGER
14 static int lps22df_trig_cnt;
15
lps22df_trigger_handler(const struct device * dev,const struct sensor_trigger * trig)16 static void lps22df_trigger_handler(const struct device *dev,
17 const struct sensor_trigger *trig)
18 {
19 sensor_sample_fetch_chan(dev, SENSOR_CHAN_ALL);
20 lps22df_trig_cnt++;
21 }
22 #endif
23
24 #ifdef CONFIG_LIS2MDL_TRIGGER
25 static int lis2mdl_trig_cnt;
26
lis2mdl_trigger_handler(const struct device * dev,const struct sensor_trigger * trig)27 static void lis2mdl_trigger_handler(const struct device *dev,
28 const struct sensor_trigger *trig)
29 {
30 sensor_sample_fetch_chan(dev, SENSOR_CHAN_ALL);
31 lis2mdl_trig_cnt++;
32 }
33 #endif
34
35 #ifdef CONFIG_LSM6DSO16IS_TRIGGER
36 static int lsm6dso16is_acc_trig_cnt;
37
lsm6dso16is_acc_trig_handler(const struct device * dev,const struct sensor_trigger * trig)38 static void lsm6dso16is_acc_trig_handler(const struct device *dev,
39 const struct sensor_trigger *trig)
40 {
41 sensor_sample_fetch_chan(dev, SENSOR_CHAN_ALL);
42 lsm6dso16is_acc_trig_cnt++;
43 }
44 #endif
45
46 #ifdef CONFIG_LSM6DSV16X_TRIGGER
47 static int lsm6dsv16x_acc_trig_cnt;
48
lsm6dsv16x_acc_trig_handler(const struct device * dev,const struct sensor_trigger * trig)49 static void lsm6dsv16x_acc_trig_handler(const struct device *dev,
50 const struct sensor_trigger *trig)
51 {
52 sensor_sample_fetch_chan(dev, SENSOR_CHAN_ALL);
53 lsm6dsv16x_acc_trig_cnt++;
54 }
55 #endif
56
57 #ifdef CONFIG_LIS2DUX12_TRIGGER
58 static int lis2duxs12_acc_trig_cnt;
59
lis2duxs12_acc_trig_handler(const struct device * dev,const struct sensor_trigger * trig)60 static void lis2duxs12_acc_trig_handler(const struct device *dev,
61 const struct sensor_trigger *trig)
62 {
63 sensor_sample_fetch_chan(dev, SENSOR_CHAN_ALL);
64 lis2duxs12_acc_trig_cnt++;
65 }
66 #endif
67
lis2mdl_config(const struct device * lis2mdl)68 static void lis2mdl_config(const struct device *lis2mdl)
69 {
70 struct sensor_value odr_attr;
71
72 /* set LIS2MDL sampling frequency to 100 Hz */
73 odr_attr.val1 = 100;
74 odr_attr.val2 = 0;
75
76 if (sensor_attr_set(lis2mdl, SENSOR_CHAN_ALL,
77 SENSOR_ATTR_SAMPLING_FREQUENCY, &odr_attr) < 0) {
78 printk("Cannot set sampling frequency for LIS2MDL\n");
79 return;
80 }
81
82 #ifdef CONFIG_LIS2MDL_TRIGGER
83 struct sensor_trigger trig;
84
85 trig.type = SENSOR_TRIG_DATA_READY;
86 trig.chan = SENSOR_CHAN_MAGN_XYZ;
87 sensor_trigger_set(lis2mdl, &trig, lis2mdl_trigger_handler);
88 #endif
89 }
90
lsm6dso16is_config(const struct device * lsm6dso16is)91 static void lsm6dso16is_config(const struct device *lsm6dso16is)
92 {
93 struct sensor_value odr_attr, fs_attr, mode_attr;
94
95 mode_attr.val1 = 0; /* HP */
96
97 if (sensor_attr_set(lsm6dso16is, SENSOR_CHAN_ACCEL_XYZ,
98 SENSOR_ATTR_CONFIGURATION, &mode_attr) < 0) {
99 printk("Cannot set mode for LSM6DSO16IS accel\n");
100 return;
101 }
102
103 /* set LSM6DSO16IS accel sampling frequency to 208 Hz */
104 odr_attr.val1 = 208;
105 odr_attr.val2 = 0;
106
107 if (sensor_attr_set(lsm6dso16is, SENSOR_CHAN_ACCEL_XYZ,
108 SENSOR_ATTR_SAMPLING_FREQUENCY, &odr_attr) < 0) {
109 printk("Cannot set sampling frequency for LSM6DSO16IS accel\n");
110 return;
111 }
112
113 sensor_g_to_ms2(16, &fs_attr);
114
115 if (sensor_attr_set(lsm6dso16is, SENSOR_CHAN_ACCEL_XYZ,
116 SENSOR_ATTR_FULL_SCALE, &fs_attr) < 0) {
117 printk("Cannot set full scale for LSM6DSO16IS accel\n");
118 return;
119 }
120
121 /* set LSM6DSO16IS gyro sampling frequency to 208 Hz */
122 odr_attr.val1 = 208;
123 odr_attr.val2 = 0;
124
125 if (sensor_attr_set(lsm6dso16is, SENSOR_CHAN_GYRO_XYZ,
126 SENSOR_ATTR_SAMPLING_FREQUENCY, &odr_attr) < 0) {
127 printk("Cannot set sampling frequency for LSM6DSO16IS gyro\n");
128 return;
129 }
130
131 sensor_degrees_to_rad(250, &fs_attr);
132
133 if (sensor_attr_set(lsm6dso16is, SENSOR_CHAN_GYRO_XYZ,
134 SENSOR_ATTR_FULL_SCALE, &fs_attr) < 0) {
135 printk("Cannot set full scale for LSM6DSO16IS gyro\n");
136 return;
137 }
138
139 #ifdef CONFIG_LSM6DSO16IS_TRIGGER
140 struct sensor_trigger trig;
141
142 trig.type = SENSOR_TRIG_DATA_READY;
143 trig.chan = SENSOR_CHAN_ACCEL_XYZ;
144 sensor_trigger_set(lsm6dso16is, &trig, lsm6dso16is_acc_trig_handler);
145 #endif
146 }
147
lsm6dsv16x_config(const struct device * lsm6dsv16x)148 static void lsm6dsv16x_config(const struct device *lsm6dsv16x)
149 {
150 struct sensor_value odr_attr, fs_attr, mode_attr;
151
152 mode_attr.val1 = 0; /* HP */
153
154 if (sensor_attr_set(lsm6dsv16x, SENSOR_CHAN_ACCEL_XYZ,
155 SENSOR_ATTR_CONFIGURATION, &mode_attr) < 0) {
156 printk("Cannot set mode for LSM6DSV16X accel\n");
157 return;
158 }
159
160 /* set LSM6DSV16X accel sampling frequency to 208 Hz */
161 odr_attr.val1 = 208;
162 odr_attr.val2 = 0;
163
164 if (sensor_attr_set(lsm6dsv16x, SENSOR_CHAN_ACCEL_XYZ,
165 SENSOR_ATTR_SAMPLING_FREQUENCY, &odr_attr) < 0) {
166 printk("Cannot set sampling frequency for LSM6DSV16X accel\n");
167 return;
168 }
169
170 sensor_g_to_ms2(16, &fs_attr);
171
172 if (sensor_attr_set(lsm6dsv16x, SENSOR_CHAN_ACCEL_XYZ,
173 SENSOR_ATTR_FULL_SCALE, &fs_attr) < 0) {
174 printk("Cannot set full scale for LSM6DSV16X accel\n");
175 return;
176 }
177
178 /* set LSM6DSV16X gyro sampling frequency to 208 Hz */
179 odr_attr.val1 = 208;
180 odr_attr.val2 = 0;
181
182 if (sensor_attr_set(lsm6dsv16x, SENSOR_CHAN_GYRO_XYZ,
183 SENSOR_ATTR_SAMPLING_FREQUENCY, &odr_attr) < 0) {
184 printk("Cannot set sampling frequency for LSM6DSV16X gyro\n");
185 return;
186 }
187
188 sensor_degrees_to_rad(250, &fs_attr);
189
190 if (sensor_attr_set(lsm6dsv16x, SENSOR_CHAN_GYRO_XYZ,
191 SENSOR_ATTR_FULL_SCALE, &fs_attr) < 0) {
192 printk("Cannot set full scale for LSM6DSV16X gyro\n");
193 return;
194 }
195
196 #ifdef CONFIG_LSM6DSV16X_TRIGGER
197 struct sensor_trigger trig;
198
199 trig.type = SENSOR_TRIG_DATA_READY;
200 trig.chan = SENSOR_CHAN_ACCEL_XYZ;
201 sensor_trigger_set(lsm6dsv16x, &trig, lsm6dsv16x_acc_trig_handler);
202 #endif
203 }
204
lps22df_config(const struct device * lps22df)205 static void lps22df_config(const struct device *lps22df)
206 {
207 struct sensor_value odr_attr;
208
209 /* set LPS22DF accel sampling frequency to 10 Hz */
210 odr_attr.val1 = 10;
211 odr_attr.val2 = 0;
212
213 if (sensor_attr_set(lps22df, SENSOR_CHAN_ALL,
214 SENSOR_ATTR_SAMPLING_FREQUENCY, &odr_attr) < 0) {
215 printk("Cannot set sampling frequency for LPS22DF accel\n");
216 return;
217 }
218
219 #ifdef CONFIG_LPS2XDF_TRIGGER
220 struct sensor_trigger trig;
221
222 trig.type = SENSOR_TRIG_DATA_READY;
223 trig.chan = SENSOR_CHAN_ALL;
224 sensor_trigger_set(lps22df, &trig, lps22df_trigger_handler);
225 #endif
226 }
227
lis2duxs12_config(const struct device * lis2duxs12)228 static void lis2duxs12_config(const struct device *lis2duxs12)
229 {
230 struct sensor_value odr_attr, fs_attr;
231
232 /* set LSM6DSV16X accel sampling frequency to 200 Hz */
233 odr_attr.val1 = 200;
234 odr_attr.val2 = 0;
235
236 if (sensor_attr_set(lis2duxs12, SENSOR_CHAN_ACCEL_XYZ,
237 SENSOR_ATTR_SAMPLING_FREQUENCY, &odr_attr) < 0) {
238 printk("Cannot set sampling frequency for LSM6DSV16X accel\n");
239 return;
240 }
241
242 sensor_g_to_ms2(16, &fs_attr);
243
244 if (sensor_attr_set(lis2duxs12, SENSOR_CHAN_ACCEL_XYZ,
245 SENSOR_ATTR_FULL_SCALE, &fs_attr) < 0) {
246 printk("Cannot set fs for LSM6DSV16X accel\n");
247 return;
248 }
249
250 #ifdef CONFIG_LIS2DUX12_TRIGGER
251 struct sensor_trigger trig;
252
253 trig.type = SENSOR_TRIG_DATA_READY;
254 trig.chan = SENSOR_CHAN_ACCEL_XYZ;
255 sensor_trigger_set(lis2duxs12, &trig, lis2duxs12_acc_trig_handler);
256 #endif
257 }
258
main(void)259 int main(void)
260 {
261 struct sensor_value lis2mdl_magn[3], lis2mdl_temp, lps22df_press, lps22df_temp;
262 struct sensor_value lsm6dso16is_xl[3], lsm6dso16is_gy[3];
263 #ifdef CONFIG_LSM6DSO16IS_ENABLE_TEMP
264 struct sensor_value lsm6dso16is_temp;
265 #endif
266 #ifdef CONFIG_LSM6DSV16X_ENABLE_TEMP
267 struct sensor_value lsm6dsv16x_temp;
268 #endif
269 struct sensor_value lsm6dsv16x_xl[3], lsm6dsv16x_gy[3];
270 struct sensor_value lis2duxs12_xl[3], lis2duxs12_temp;
271 const struct device *const lis2mdl = DEVICE_DT_GET_ONE(st_lis2mdl);
272 const struct device *const lsm6dso16is = DEVICE_DT_GET_ONE(st_lsm6dso16is);
273 const struct device *const lsm6dsv16x = DEVICE_DT_GET_ONE(st_lsm6dsv16x);
274 const struct device *const lps22df = DEVICE_DT_GET_ONE(st_lps22df);
275 const struct device *const lis2duxs12 = DEVICE_DT_GET_ONE(st_lis2duxs12);
276 int cnt = 1;
277
278 if (!device_is_ready(lsm6dso16is)) {
279 printk("%s: device not ready.\n", lsm6dso16is->name);
280 return 0;
281 }
282 if (!device_is_ready(lsm6dsv16x)) {
283 printk("%s: device not ready.\n", lsm6dsv16x->name);
284 return 0;
285 }
286 if (!device_is_ready(lis2mdl)) {
287 printk("%s: device not ready.\n", lis2mdl->name);
288 return 0;
289 }
290 if (!device_is_ready(lps22df)) {
291 printk("%s: device not ready.\n", lps22df->name);
292 return 0;
293 }
294 if (!device_is_ready(lis2duxs12)) {
295 printk("%s: device not ready.\n", lis2duxs12->name);
296 return 0;
297 }
298
299 lis2mdl_config(lis2mdl);
300 lsm6dso16is_config(lsm6dso16is);
301 lsm6dsv16x_config(lsm6dsv16x);
302 lps22df_config(lps22df);
303 lis2duxs12_config(lis2duxs12);
304
305 while (1) {
306 /* Get sensor samples */
307
308 #ifndef CONFIG_LIS2MDL_TRIGGER
309 if (sensor_sample_fetch(lis2mdl) < 0) {
310 printf("LIS2MDL Magn Sensor sample update error\n");
311 return 0;
312 }
313 #endif
314 #ifndef CONFIG_LSM6DSO16IS_TRIGGER
315 if (sensor_sample_fetch(lsm6dso16is) < 0) {
316 printf("LSM6DSO16IS Sensor sample update error\n");
317 return 0;
318 }
319 #endif
320 #ifndef CONFIG_LSM6DSV16X_TRIGGER
321 if (sensor_sample_fetch(lsm6dsv16x) < 0) {
322 printf("LSM6DSV16X Sensor sample update error\n");
323 return 0;
324 }
325 #endif
326 #ifndef CONFIG_LPS2XDF_TRIGGER
327 if (sensor_sample_fetch(lps22df) < 0) {
328 printf("LPS22DF pressure sample update error\n");
329 return 0;
330 }
331 #endif
332 #ifndef CONFIG_LIS2DUX12_TRIGGER
333 if (sensor_sample_fetch(lis2duxs12) < 0) {
334 printf("LIS2DUXS12 XL Sensor sample update error\n");
335 return 0;
336 }
337 #endif
338
339 /* Get sensor data */
340 sensor_channel_get(lis2mdl, SENSOR_CHAN_MAGN_XYZ, lis2mdl_magn);
341 sensor_channel_get(lis2mdl, SENSOR_CHAN_DIE_TEMP, &lis2mdl_temp);
342 sensor_channel_get(lsm6dso16is, SENSOR_CHAN_ACCEL_XYZ, lsm6dso16is_xl);
343 sensor_channel_get(lsm6dso16is, SENSOR_CHAN_GYRO_XYZ, lsm6dso16is_gy);
344 #ifdef CONFIG_LSM6DSO16IS_ENABLE_TEMP
345 sensor_channel_get(lsm6dso16is, SENSOR_CHAN_DIE_TEMP, &lsm6dso16is_temp);
346 #endif
347 #ifdef CONFIG_LSM6DSV16X_ENABLE_TEMP
348 sensor_channel_get(lsm6dsv16x, SENSOR_CHAN_DIE_TEMP, &lsm6dsv16x_temp);
349 #endif
350 sensor_channel_get(lsm6dsv16x, SENSOR_CHAN_ACCEL_XYZ, lsm6dsv16x_xl);
351 sensor_channel_get(lsm6dsv16x, SENSOR_CHAN_GYRO_XYZ, lsm6dsv16x_gy);
352
353 sensor_channel_get(lps22df, SENSOR_CHAN_PRESS, &lps22df_press);
354 sensor_channel_get(lps22df, SENSOR_CHAN_AMBIENT_TEMP, &lps22df_temp);
355
356 sensor_channel_get(lis2duxs12, SENSOR_CHAN_ACCEL_XYZ, lis2duxs12_xl);
357 sensor_channel_get(lis2duxs12, SENSOR_CHAN_DIE_TEMP, &lis2duxs12_temp);
358 /* Display sensor data */
359
360 /* Erase previous */
361 printf("\0033\014");
362
363 printf("X-NUCLEO-IKS4A1 sensor dashboard\n\n");
364
365 /* lis2mdl */
366 printf("LIS2MDL: Magn (gauss): x: %.3f, y: %.3f, z: %.3f\n",
367 sensor_value_to_double(&lis2mdl_magn[0]),
368 sensor_value_to_double(&lis2mdl_magn[1]),
369 sensor_value_to_double(&lis2mdl_magn[2]));
370
371 printf("LIS2MDL: Temperature: %.1f C\n",
372 sensor_value_to_double(&lis2mdl_temp));
373
374 printf("LSM6DSO16IS: Accel (m.s-2): x: %.3f, y: %.3f, z: %.3f\n",
375 sensor_value_to_double(&lsm6dso16is_xl[0]),
376 sensor_value_to_double(&lsm6dso16is_xl[1]),
377 sensor_value_to_double(&lsm6dso16is_xl[2]));
378
379 printf("LSM6DSO16IS: Gyro (dps): x: %.3f, y: %.3f, z: %.3f\n",
380 sensor_value_to_double(&lsm6dso16is_gy[0]),
381 sensor_value_to_double(&lsm6dso16is_gy[1]),
382 sensor_value_to_double(&lsm6dso16is_gy[2]));
383
384 #ifdef CONFIG_LSM6DSO16IS_ENABLE_TEMP
385 /* temperature */
386 printf("LSM6DSO16IS: Temperature: %.1f C\n",
387 sensor_value_to_double(&lsm6dso16is_temp));
388 #endif
389
390 printf("LSM6DSV16X: Accel (m.s-2): x: %.3f, y: %.3f, z: %.3f\n",
391 sensor_value_to_double(&lsm6dsv16x_xl[0]),
392 sensor_value_to_double(&lsm6dsv16x_xl[1]),
393 sensor_value_to_double(&lsm6dsv16x_xl[2]));
394
395 printf("LSM6DSV16X: GYro (dps): x: %.3f, y: %.3f, z: %.3f\n",
396 sensor_value_to_double(&lsm6dsv16x_gy[0]),
397 sensor_value_to_double(&lsm6dsv16x_gy[1]),
398 sensor_value_to_double(&lsm6dsv16x_gy[2]));
399
400 #ifdef CONFIG_LSM6DSV16X_ENABLE_TEMP
401 /* temperature */
402 printf("LSM6DSV16X: Temperature: %.1f C\n",
403 sensor_value_to_double(&lsm6dsv16x_temp));
404 #endif
405
406 printf("LPS22DF: Temperature: %.1f C\n", sensor_value_to_double(&lps22df_temp));
407 printf("LPS22DF: Pressure:%.3f kpa\n", sensor_value_to_double(&lps22df_press));
408
409 printf("LIS2DUXS12: Accel (m.s-2): x: %.3f, y: %.3f, z: %.3f\n",
410 sensor_value_to_double(&lis2duxs12_xl[0]),
411 sensor_value_to_double(&lis2duxs12_xl[1]),
412 sensor_value_to_double(&lis2duxs12_xl[2]));
413
414 printf("LIS2DUXS12: Temperature: %.1f C\n",
415 sensor_value_to_double(&lis2duxs12_temp));
416
417 #if defined(CONFIG_LIS2MDL_TRIGGER)
418 printk("%d: lis2mdl trig %d\n", cnt, lis2mdl_trig_cnt);
419 #endif
420
421 #ifdef CONFIG_LSM6DSO16IS_TRIGGER
422 printk("%d: lsm6dso16is acc trig %d\n", cnt, lsm6dso16is_acc_trig_cnt);
423 #endif
424
425 #ifdef CONFIG_LSM6DSV16X_TRIGGER
426 printk("%d: lsm6dsv16x acc trig %d\n", cnt, lsm6dsv16x_acc_trig_cnt);
427 #endif
428 #ifdef CONFIG_LPS2XDF_TRIGGER
429 printk("%d: lps22df trig %d\n", cnt, lps22df_trig_cnt);
430 #endif
431 #ifdef CONFIG_LIS2DUX12_TRIGGER
432 printk("%d:: lis2duxs12 acc trig %d\n", cnt, lis2duxs12_acc_trig_cnt);
433 #endif
434
435 cnt++;
436 k_sleep(K_MSEC(2000));
437 }
438 }
439
