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_LSM6DSV16X_TRIGGER
14 static int lsm6dsv16x_acc_trig_cnt;
15
lsm6dsv16x_acc_trig_handler(const struct device * dev,const struct sensor_trigger * trig)16 static void lsm6dsv16x_acc_trig_handler(const struct device *dev,
17 const struct sensor_trigger *trig)
18 {
19 sensor_sample_fetch_chan(dev, SENSOR_CHAN_ALL);
20 lsm6dsv16x_acc_trig_cnt++;
21 }
22 #endif
23
lsm6dsv16x_config(const struct device * lsm6dsv16x)24 static void lsm6dsv16x_config(const struct device *lsm6dsv16x)
25 {
26 struct sensor_value odr_attr, fs_attr;
27
28 /* set LSM6DSV16X accel sampling frequency to 208 Hz */
29 odr_attr.val1 = 208;
30 odr_attr.val2 = 0;
31
32 if (sensor_attr_set(lsm6dsv16x, SENSOR_CHAN_ACCEL_XYZ,
33 SENSOR_ATTR_SAMPLING_FREQUENCY, &odr_attr) < 0) {
34 printk("Cannot set sampling frequency for LSM6DSV16X accel\n");
35 return;
36 }
37
38 sensor_g_to_ms2(16, &fs_attr);
39
40 if (sensor_attr_set(lsm6dsv16x, SENSOR_CHAN_ACCEL_XYZ,
41 SENSOR_ATTR_FULL_SCALE, &fs_attr) < 0) {
42 printk("Cannot set full scale for LSM6DSV16X accel\n");
43 return;
44 }
45
46 /* set LSM6DSV16X gyro sampling frequency to 208 Hz */
47 odr_attr.val1 = 208;
48 odr_attr.val2 = 0;
49
50 if (sensor_attr_set(lsm6dsv16x, SENSOR_CHAN_GYRO_XYZ,
51 SENSOR_ATTR_SAMPLING_FREQUENCY, &odr_attr) < 0) {
52 printk("Cannot set sampling frequency for LSM6DSV16X gyro\n");
53 return;
54 }
55
56 sensor_degrees_to_rad(250, &fs_attr);
57
58 if (sensor_attr_set(lsm6dsv16x, SENSOR_CHAN_GYRO_XYZ,
59 SENSOR_ATTR_FULL_SCALE, &fs_attr) < 0) {
60 printk("Cannot set full scale for LSM6DSV16X gyro\n");
61 return;
62 }
63
64 /* set LSM6DSV16X external magn sampling frequency to 100 Hz */
65 odr_attr.val1 = 100;
66 odr_attr.val2 = 0;
67
68 #ifdef CONFIG_LSM6DSV16X_EXT_LIS2MDL
69 if (sensor_attr_set(lsm6dsv16x, SENSOR_CHAN_MAGN_XYZ,
70 SENSOR_ATTR_SAMPLING_FREQUENCY, &odr_attr) < 0) {
71 printk("Cannot set sampling frequency for LSM6DSV16X ext magn\n");
72 }
73 #endif
74
75 #ifdef CONFIG_LSM6DSV16X_EXT_LPS22DF
76 if (sensor_attr_set(lsm6dsv16x, SENSOR_CHAN_PRESS,
77 SENSOR_ATTR_SAMPLING_FREQUENCY, &odr_attr) < 0) {
78 printk("Cannot set sampling frequency for LSM6DSV16X ext pressure\n");
79 }
80 #endif
81
82 #ifdef CONFIG_LSM6DSV16X_EXT_HTS221
83 odr_attr.val1 = 12;
84 if (sensor_attr_set(lsm6dsv16x, SENSOR_CHAN_HUMIDITY,
85 SENSOR_ATTR_SAMPLING_FREQUENCY, &odr_attr) < 0) {
86 printk("Cannot set sampling frequency for LSM6DSV16X ext humidity\n");
87 }
88 #endif
89
90 #ifdef CONFIG_LSM6DSV16X_TRIGGER
91 struct sensor_trigger trig;
92
93 trig.type = SENSOR_TRIG_DATA_READY;
94 trig.chan = SENSOR_CHAN_ACCEL_XYZ;
95 sensor_trigger_set(lsm6dsv16x, &trig, lsm6dsv16x_acc_trig_handler);
96 #endif
97 }
98
main(void)99 int main(void)
100 {
101 struct sensor_value lsm6dsv16x_xl[3], lsm6dsv16x_gy[3];
102 #ifdef CONFIG_LSM6DSV16X_ENABLE_TEMP
103 struct sensor_value lsm6dsv16x_temp;
104 #endif
105 #ifdef CONFIG_LSM6DSV16X_EXT_LIS2MDL
106 struct sensor_value lis2mdl_magn[3];
107 #endif
108 #ifdef CONFIG_LSM6DSV16X_EXT_LPS22DF
109 struct sensor_value lps22df_press;
110 struct sensor_value lps22df_temp;
111 #endif
112 const struct device *const lsm6dsv16x = DEVICE_DT_GET_ONE(st_lsm6dsv16x);
113 int cnt = 1;
114
115 if (!device_is_ready(lsm6dsv16x)) {
116 printk("%s: device not ready.\n", lsm6dsv16x->name);
117 return 0;
118 }
119
120 lsm6dsv16x_config(lsm6dsv16x);
121
122 while (1) {
123 /* Get sensor samples */
124 #ifndef CONFIG_LSM6DSV16X_TRIGGER
125 if (sensor_sample_fetch(lsm6dsv16x) < 0) {
126 printf("LSM6DSV16X Sensor sample update error\n");
127 return 0;
128 }
129 #endif
130
131 /* Get sensor data */
132 sensor_channel_get(lsm6dsv16x, SENSOR_CHAN_ACCEL_XYZ, lsm6dsv16x_xl);
133 sensor_channel_get(lsm6dsv16x, SENSOR_CHAN_GYRO_XYZ, lsm6dsv16x_gy);
134 #ifdef CONFIG_LSM6DSV16X_ENABLE_TEMP
135 sensor_channel_get(lsm6dsv16x, SENSOR_CHAN_DIE_TEMP, &lsm6dsv16x_temp);
136 #endif
137 #ifdef CONFIG_LSM6DSV16X_EXT_LIS2MDL
138 sensor_channel_get(lsm6dsv16x, SENSOR_CHAN_MAGN_XYZ, lis2mdl_magn);
139 #endif
140 #ifdef CONFIG_LSM6DSV16X_EXT_LPS22DF
141 sensor_channel_get(lsm6dsv16x, SENSOR_CHAN_AMBIENT_TEMP, &lps22df_temp);
142 sensor_channel_get(lsm6dsv16x, SENSOR_CHAN_PRESS, &lps22df_press);
143 #endif
144
145 /* Display sensor data */
146
147 /* Erase previous */
148 printf("\0033\014");
149
150 printf("X-NUCLEO-IKS01A4 sensor dashboard\n\n");
151
152 printf("LSM6DSV16X: Accel (m.s-2): x: %.3f, y: %.3f, z: %.3f\n",
153 sensor_value_to_double(&lsm6dsv16x_xl[0]),
154 sensor_value_to_double(&lsm6dsv16x_xl[1]),
155 sensor_value_to_double(&lsm6dsv16x_xl[2]));
156
157 printf("LSM6DSV16X: Gyro (dps): x: %.3f, y: %.3f, z: %.3f\n",
158 sensor_value_to_double(&lsm6dsv16x_gy[0]),
159 sensor_value_to_double(&lsm6dsv16x_gy[1]),
160 sensor_value_to_double(&lsm6dsv16x_gy[2]));
161
162 #ifdef CONFIG_LSM6DSV16X_ENABLE_TEMP
163 /* temperature */
164 printf("LSM6DSV16X: Temperature: %.1f C\n",
165 sensor_value_to_double(&lsm6dsv16x_temp));
166 #endif
167
168 #ifdef CONFIG_LSM6DSV16X_EXT_LIS2MDL
169 printf("LSM6DSV16X: Magn (gauss): x: %.3f, y: %.3f, z: %.3f\n",
170 sensor_value_to_double(&lis2mdl_magn[0]),
171 sensor_value_to_double(&lis2mdl_magn[1]),
172 sensor_value_to_double(&lis2mdl_magn[2]));
173 #endif
174
175 #ifdef CONFIG_LSM6DSV16X_EXT_LPS22DF
176 printf("LSM6DSV16X: Temperature: %.1f C\n",
177 sensor_value_to_double(&lps22df_temp));
178
179 printf("LSM6DSV16X: Pressure:%.3f kpa\n",
180 sensor_value_to_double(&lps22df_press));
181 #endif
182
183 #ifdef CONFIG_LSM6DSV16X_TRIGGER
184 printk("%d: lsm6dsv16x acc trig %d\n", cnt, lsm6dsv16x_acc_trig_cnt);
185 #endif
186
187 cnt++;
188 k_sleep(K_MSEC(2000));
189 }
190 }
191
