1 /*
2 * Copyright (c) 2020 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_LSM6DSL_TRIGGER
14 static int lsm6dsl_trig_cnt;
15
lsm6dsl_trigger_handler(const struct device * dev,const struct sensor_trigger * trig)16 static void lsm6dsl_trigger_handler(const struct device *dev,
17 const struct sensor_trigger *trig)
18 {
19 sensor_sample_fetch_chan(dev, SENSOR_CHAN_ALL);
20 lsm6dsl_trig_cnt++;
21 }
22 #endif
23
main(void)24 int main(void)
25 {
26 #ifdef CONFIG_LSM6DSL_TRIGGER
27 int cnt = 1;
28 #endif
29 #ifdef CONFIG_LSM6DSL_EXT0_LPS22HB
30 struct sensor_value temp, press;
31 #endif
32 #ifdef CONFIG_LSM6DSL_EXT0_LIS2MDL
33 struct sensor_value magn[3];
34 #endif
35 struct sensor_value accel[3];
36 struct sensor_value gyro[3];
37 const struct device *const lsm6dsl = DEVICE_DT_GET_ONE(st_lsm6dsl);
38
39 if (!device_is_ready(lsm6dsl)) {
40 printk("%s: device not ready.\n", lsm6dsl->name);
41 return 0;
42 }
43
44 /* set LSM6DSL accel/gyro sampling frequency to 104 Hz */
45 struct sensor_value odr_attr;
46
47 odr_attr.val1 = 104;
48 odr_attr.val2 = 0;
49
50 if (sensor_attr_set(lsm6dsl, SENSOR_CHAN_ACCEL_XYZ,
51 SENSOR_ATTR_SAMPLING_FREQUENCY, &odr_attr) < 0) {
52 printk("Cannot set sampling frequency for accelerometer.\n");
53 return 0;
54 }
55
56 if (sensor_attr_set(lsm6dsl, SENSOR_CHAN_GYRO_XYZ,
57 SENSOR_ATTR_SAMPLING_FREQUENCY, &odr_attr) < 0) {
58 printk("Cannot set sampling frequency for gyro.\n");
59 return 0;
60 }
61
62 #ifdef CONFIG_LSM6DSL_TRIGGER
63 struct sensor_trigger trig;
64
65 trig.type = SENSOR_TRIG_DATA_READY;
66 trig.chan = SENSOR_CHAN_ACCEL_XYZ;
67 sensor_trigger_set(lsm6dsl, &trig, lsm6dsl_trigger_handler);
68 #endif
69
70 while (1) {
71 /* Get sensor samples */
72
73 #ifndef CONFIG_LSM6DSL_TRIGGER
74 if (sensor_sample_fetch(lsm6dsl) < 0) {
75 printf("LSM6DSL Sensor sample update error\n");
76 return 0;
77 }
78 #endif
79
80 /* Get sensor data */
81
82 sensor_channel_get(lsm6dsl, SENSOR_CHAN_ACCEL_XYZ, accel);
83 sensor_channel_get(lsm6dsl, SENSOR_CHAN_GYRO_XYZ, gyro);
84 #ifdef CONFIG_LSM6DSL_EXT0_LPS22HB
85 sensor_channel_get(lsm6dsl, SENSOR_CHAN_AMBIENT_TEMP, &temp);
86 sensor_channel_get(lsm6dsl, SENSOR_CHAN_PRESS, &press);
87 #endif
88 #ifdef CONFIG_LSM6DSL_EXT0_LIS2MDL
89 sensor_channel_get(lsm6dsl, SENSOR_CHAN_MAGN_XYZ, magn);
90 #endif
91
92 /* Display sensor data */
93
94 /* Erase previous */
95 printf("\0033\014");
96
97 printf("X-NUCLEO-IKS01A2 sensor dashboard\n\n");
98
99 /* lsm6dsl accel */
100 printf("LSM6DSL: Accel (m.s-2): x: %.1f, y: %.1f, z: %.1f\n",
101 sensor_value_to_double(&accel[0]),
102 sensor_value_to_double(&accel[1]),
103 sensor_value_to_double(&accel[2]));
104
105 /* lsm6dsl gyro */
106 printf("LSM6DSL: Gyro (dps): x: %.3f, y: %.3f, z: %.3f\n",
107 sensor_value_to_double(&gyro[0]),
108 sensor_value_to_double(&gyro[1]),
109 sensor_value_to_double(&gyro[2]));
110
111 #ifdef CONFIG_LSM6DSL_EXT0_LPS22HB
112 printf("LSM6DSL: Temperature: %.1f C\n",
113 sensor_value_to_double(&temp));
114
115 printf("LSM6DSL: Pressure:%.3f kpa\n",
116 sensor_value_to_double(&press));
117 #endif
118
119 #ifdef CONFIG_LSM6DSL_EXT0_LIS2MDL
120 printf("LSM6DSL: Magn (gauss): x: %.3f, y: %.3f, z: %.3f\n",
121 sensor_value_to_double(&magn[0]),
122 sensor_value_to_double(&magn[1]),
123 sensor_value_to_double(&magn[2]));
124 #endif
125
126 #ifdef CONFIG_LSM6DSL_TRIGGER
127 printk("%d:: lsm6dsl acc trig %d\n", cnt++, lsm6dsl_trig_cnt);
128 #endif
129
130 k_sleep(K_MSEC(2000));
131 }
132 }
133
