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