1 /*
2 * Copyright (c) 2019 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_LIS2DW12_TRIGGER
14 static int lis2dw12_trig_cnt;
15
lis2dw12_trigger_handler(const struct device * dev,const struct sensor_trigger * trig)16 static void lis2dw12_trigger_handler(const struct device *dev,
17 const struct sensor_trigger *trig)
18 {
19 sensor_sample_fetch_chan(dev, SENSOR_CHAN_ACCEL_XYZ);
20 lis2dw12_trig_cnt++;
21 }
22 #endif
23
24 #ifdef CONFIG_LSM6DSO_TRIGGER
25 static int lsm6dso_acc_trig_cnt;
26
lsm6dso_acc_trig_handler(const struct device * dev,const struct sensor_trigger * trig)27 static void lsm6dso_acc_trig_handler(const struct device *dev,
28 const struct sensor_trigger *trig)
29 {
30 sensor_sample_fetch(dev);
31 lsm6dso_acc_trig_cnt++;
32 }
33 #endif
34
lis2dw12_config(const struct device * lis2dw12)35 static void lis2dw12_config(const struct device *lis2dw12)
36 {
37 struct sensor_value odr_attr, fs_attr;
38
39 /* set LIS2DW12 accel/gyro sampling frequency to 100 Hz */
40 odr_attr.val1 = 100;
41 odr_attr.val2 = 0;
42
43 if (sensor_attr_set(lis2dw12, SENSOR_CHAN_ACCEL_XYZ,
44 SENSOR_ATTR_SAMPLING_FREQUENCY, &odr_attr) < 0) {
45 printk("Cannot set sampling frequency for LIS2DW12 accel\n");
46 return;
47 }
48
49 sensor_g_to_ms2(16, &fs_attr);
50
51 if (sensor_attr_set(lis2dw12, SENSOR_CHAN_ACCEL_XYZ,
52 SENSOR_ATTR_FULL_SCALE, &fs_attr) < 0) {
53 printk("Cannot set sampling frequency for LIS2DW12 gyro\n");
54 return;
55 }
56
57 #ifdef CONFIG_LIS2DW12_TRIGGER
58 struct sensor_trigger trig;
59
60 trig.type = SENSOR_TRIG_DATA_READY;
61 trig.chan = SENSOR_CHAN_ACCEL_XYZ;
62 sensor_trigger_set(lis2dw12, &trig, lis2dw12_trigger_handler);
63 #endif
64 }
65
lsm6dso_config(const struct device * lsm6dso)66 static void lsm6dso_config(const struct device *lsm6dso)
67 {
68 struct sensor_value odr_attr, fs_attr;
69
70 /* set LSM6DSO accel sampling frequency to 208 Hz */
71 odr_attr.val1 = 208;
72 odr_attr.val2 = 0;
73
74 if (sensor_attr_set(lsm6dso, SENSOR_CHAN_ACCEL_XYZ,
75 SENSOR_ATTR_SAMPLING_FREQUENCY, &odr_attr) < 0) {
76 printk("Cannot set sampling frequency for LSM6DSO accel\n");
77 return;
78 }
79
80 sensor_g_to_ms2(16, &fs_attr);
81
82 if (sensor_attr_set(lsm6dso, SENSOR_CHAN_ACCEL_XYZ,
83 SENSOR_ATTR_FULL_SCALE, &fs_attr) < 0) {
84 printk("Cannot set fs for LSM6DSO accel\n");
85 return;
86 }
87
88 /* set LSM6DSO gyro sampling frequency to 208 Hz */
89 odr_attr.val1 = 208;
90 odr_attr.val2 = 0;
91
92 if (sensor_attr_set(lsm6dso, SENSOR_CHAN_GYRO_XYZ,
93 SENSOR_ATTR_SAMPLING_FREQUENCY, &odr_attr) < 0) {
94 printk("Cannot set sampling frequency for LSM6DSO gyro\n");
95 return;
96 }
97
98 sensor_degrees_to_rad(250, &fs_attr);
99
100 if (sensor_attr_set(lsm6dso, SENSOR_CHAN_GYRO_XYZ,
101 SENSOR_ATTR_FULL_SCALE, &fs_attr) < 0) {
102 printk("Cannot set fs for LSM6DSO gyro\n");
103 return;
104 }
105
106 /* set LSM6DSO external magn sampling frequency to 100 Hz */
107 odr_attr.val1 = 100;
108 odr_attr.val2 = 0;
109
110 #ifdef CONFIG_LSM6DSO_EXT_LIS2MDL
111 if (sensor_attr_set(lsm6dso, SENSOR_CHAN_MAGN_XYZ,
112 SENSOR_ATTR_SAMPLING_FREQUENCY, &odr_attr) < 0) {
113 printk("Cannot set sampling frequency for LSM6DSO ext magn\n");
114 return;
115 }
116 #endif
117
118 #ifdef CONFIG_LSM6DSO_EXT_LPS22HH
119 if (sensor_attr_set(lsm6dso, SENSOR_CHAN_PRESS,
120 SENSOR_ATTR_SAMPLING_FREQUENCY, &odr_attr) < 0) {
121 printk("Cannot set sampling frequency for LSM6DSO ext pressure\n");
122 return;
123 }
124 #endif
125
126 #ifdef CONFIG_LSM6DSO_EXT_HTS221
127 odr_attr.val1 = 12;
128 if (sensor_attr_set(lsm6dso, SENSOR_CHAN_HUMIDITY,
129 SENSOR_ATTR_SAMPLING_FREQUENCY, &odr_attr) < 0) {
130 printk("Cannot set sampling frequency for LSM6DSO ext humidity\n");
131 return;
132 }
133 #endif
134
135 #ifdef CONFIG_LSM6DSO_TRIGGER
136 struct sensor_trigger trig;
137
138 trig.type = SENSOR_TRIG_DATA_READY;
139 trig.chan = SENSOR_CHAN_ACCEL_XYZ;
140 sensor_trigger_set(lsm6dso, &trig, lsm6dso_acc_trig_handler);
141 #endif
142 }
143
main(void)144 int main(void)
145 {
146 #ifdef CONFIG_LSM6DSO_EXT_LPS22HH
147 struct sensor_value temp2, press;
148 #endif
149 #ifdef CONFIG_LSM6DSO_EXT_HTS221
150 struct sensor_value hum;
151 #endif
152 #ifdef CONFIG_LSM6DSO_ENABLE_TEMP
153 struct sensor_value die_temp;
154 #endif
155 struct sensor_value accel1[3], accel2[3];
156 struct sensor_value gyro[3];
157 struct sensor_value magn[3];
158 const struct device *const lis2dw12 = DEVICE_DT_GET_ONE(st_lis2dw12);
159 const struct device *const lsm6dso = DEVICE_DT_GET_ONE(st_lsm6dso);
160 int cnt = 1;
161
162 if (!device_is_ready(lis2dw12)) {
163 printk("%s: device not ready.\n", lis2dw12->name);
164 return 0;
165 }
166 if (!device_is_ready(lsm6dso)) {
167 printk("%s: device not ready.\n", lsm6dso->name);
168 return 0;
169 }
170
171 lis2dw12_config(lis2dw12);
172 lsm6dso_config(lsm6dso);
173
174 while (1) {
175 /* Get sensor samples */
176
177 #ifndef CONFIG_LIS2DW12_TRIGGER
178 if (sensor_sample_fetch(lis2dw12) < 0) {
179 printf("LIS2DW12 Sensor sample update error\n");
180 return 0;
181 }
182 #endif
183 #ifndef CONFIG_LSM6DSO_TRIGGER
184 if (sensor_sample_fetch(lsm6dso) < 0) {
185 printf("LSM6DSO Sensor sample update error\n");
186 return 0;
187 }
188 #endif
189
190 /* Get sensor data */
191 sensor_channel_get(lis2dw12, SENSOR_CHAN_ACCEL_XYZ, accel2);
192 sensor_channel_get(lsm6dso, SENSOR_CHAN_ACCEL_XYZ, accel1);
193 sensor_channel_get(lsm6dso, SENSOR_CHAN_GYRO_XYZ, gyro);
194 #ifdef CONFIG_LSM6DSO_ENABLE_TEMP
195 sensor_channel_get(lsm6dso, SENSOR_CHAN_DIE_TEMP, &die_temp);
196 #endif
197 #ifdef CONFIG_LSM6DSO_EXT_LIS2MDL
198 sensor_channel_get(lsm6dso, SENSOR_CHAN_MAGN_XYZ, magn);
199 #endif
200 #ifdef CONFIG_LSM6DSO_EXT_LPS22HH
201 sensor_channel_get(lsm6dso, SENSOR_CHAN_AMBIENT_TEMP, &temp2);
202 sensor_channel_get(lsm6dso, SENSOR_CHAN_PRESS, &press);
203 #endif
204 #ifdef CONFIG_LSM6DSO_EXT_HTS221
205 sensor_channel_get(lsm6dso, SENSOR_CHAN_HUMIDITY, &hum);
206 #endif
207
208 /* Display sensor data */
209
210 /* Erase previous */
211 printf("\0033\014");
212
213 printf("X-NUCLEO-IKS01A3 sensor dashboard\n\n");
214
215 printf("LIS2DW12: Accel (m.s-2): x: %.3f, y: %.3f, z: %.3f\n",
216 sensor_value_to_double(&accel2[0]),
217 sensor_value_to_double(&accel2[1]),
218 sensor_value_to_double(&accel2[2]));
219
220 printf("LSM6DSO: Accel (m.s-2): x: %.3f, y: %.3f, z: %.3f\n",
221 sensor_value_to_double(&accel1[0]),
222 sensor_value_to_double(&accel1[1]),
223 sensor_value_to_double(&accel1[2]));
224
225 printf("LSM6DSO: GYro (dps): x: %.3f, y: %.3f, z: %.3f\n",
226 sensor_value_to_double(&gyro[0]),
227 sensor_value_to_double(&gyro[1]),
228 sensor_value_to_double(&gyro[2]));
229
230 #ifdef CONFIG_LSM6DSO_ENABLE_TEMP
231 /* temperature */
232 printf("LSM6DSO: Temperature: %.1f C\n",
233 sensor_value_to_double(&die_temp));
234 #endif
235
236 #ifdef CONFIG_LSM6DSO_EXT_LIS2MDL
237 printf("LSM6DSO: Magn (gauss): x: %.3f, y: %.3f, z: %.3f\n",
238 sensor_value_to_double(&magn[0]),
239 sensor_value_to_double(&magn[1]),
240 sensor_value_to_double(&magn[2]));
241 #endif
242
243 #ifdef CONFIG_LSM6DSO_EXT_LPS22HH
244 printf("LSM6DSO: Temperature: %.1f C\n",
245 sensor_value_to_double(&temp2));
246
247 printf("LSM6DSO: Pressure:%.3f kpa\n",
248 sensor_value_to_double(&press));
249 #endif
250
251 #ifdef CONFIG_LSM6DSO_EXT_HTS221
252 printf("LSM6DSO: Relative Humidity: %.1f%%\n",
253 sensor_value_to_double(&hum));
254
255 #endif
256
257 #ifdef CONFIG_LIS2DW12_TRIGGER
258 printk("%d:: lis2dw12 trig %d\n", cnt, lis2dw12_trig_cnt);
259 #endif
260
261 #ifdef CONFIG_LSM6DSO_TRIGGER
262 printk("%d:: lsm6dso acc trig %d\n", cnt, lsm6dso_acc_trig_cnt);
263 #endif
264
265 cnt++;
266 k_sleep(K_MSEC(2000));
267 }
268 }
269
