1 /*
2 * Copyright (c) 2018 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 static int print_samples;
14 static int lsm6dsl_trig_cnt;
15
16 static struct sensor_value accel_x_out, accel_y_out, accel_z_out;
17 static struct sensor_value gyro_x_out, gyro_y_out, gyro_z_out;
18 #if defined(CONFIG_LSM6DSL_EXT0_LIS2MDL)
19 static struct sensor_value magn_x_out, magn_y_out, magn_z_out;
20 #endif
21 #if defined(CONFIG_LSM6DSL_EXT0_LPS22HB)
22 static struct sensor_value press_out, temp_out;
23 #endif
24
25 #ifdef CONFIG_LSM6DSL_TRIGGER
lsm6dsl_trigger_handler(const struct device * dev,const struct sensor_trigger * trig)26 static void lsm6dsl_trigger_handler(const struct device *dev,
27 const struct sensor_trigger *trig)
28 {
29 static struct sensor_value accel_x, accel_y, accel_z;
30 static struct sensor_value gyro_x, gyro_y, gyro_z;
31 #if defined(CONFIG_LSM6DSL_EXT0_LIS2MDL)
32 static struct sensor_value magn_x, magn_y, magn_z;
33 #endif
34 #if defined(CONFIG_LSM6DSL_EXT0_LPS22HB)
35 static struct sensor_value press, temp;
36 #endif
37 lsm6dsl_trig_cnt++;
38
39 sensor_sample_fetch_chan(dev, SENSOR_CHAN_ACCEL_XYZ);
40 sensor_channel_get(dev, SENSOR_CHAN_ACCEL_X, &accel_x);
41 sensor_channel_get(dev, SENSOR_CHAN_ACCEL_Y, &accel_y);
42 sensor_channel_get(dev, SENSOR_CHAN_ACCEL_Z, &accel_z);
43
44 /* lsm6dsl gyro */
45 sensor_sample_fetch_chan(dev, SENSOR_CHAN_GYRO_XYZ);
46 sensor_channel_get(dev, SENSOR_CHAN_GYRO_X, &gyro_x);
47 sensor_channel_get(dev, SENSOR_CHAN_GYRO_Y, &gyro_y);
48 sensor_channel_get(dev, SENSOR_CHAN_GYRO_Z, &gyro_z);
49
50 #if defined(CONFIG_LSM6DSL_EXT0_LIS2MDL)
51 /* lsm6dsl external magn */
52 sensor_sample_fetch_chan(dev, SENSOR_CHAN_MAGN_XYZ);
53 sensor_channel_get(dev, SENSOR_CHAN_MAGN_X, &magn_x);
54 sensor_channel_get(dev, SENSOR_CHAN_MAGN_Y, &magn_y);
55 sensor_channel_get(dev, SENSOR_CHAN_MAGN_Z, &magn_z);
56 #endif
57
58 #if defined(CONFIG_LSM6DSL_EXT0_LPS22HB)
59 /* lsm6dsl external press/temp */
60 sensor_sample_fetch_chan(dev, SENSOR_CHAN_PRESS);
61 sensor_channel_get(dev, SENSOR_CHAN_PRESS, &press);
62
63 sensor_sample_fetch_chan(dev, SENSOR_CHAN_AMBIENT_TEMP);
64 sensor_channel_get(dev, SENSOR_CHAN_AMBIENT_TEMP, &temp);
65 #endif
66
67 if (print_samples) {
68 print_samples = 0;
69
70 accel_x_out = accel_x;
71 accel_y_out = accel_y;
72 accel_z_out = accel_z;
73
74 gyro_x_out = gyro_x;
75 gyro_y_out = gyro_y;
76 gyro_z_out = gyro_z;
77
78 #if defined(CONFIG_LSM6DSL_EXT0_LIS2MDL)
79 magn_x_out = magn_x;
80 magn_y_out = magn_y;
81 magn_z_out = magn_z;
82 #endif
83
84 #if defined(CONFIG_LSM6DSL_EXT0_LPS22HB)
85 press_out = press;
86 temp_out = temp;
87 #endif
88 }
89
90 }
91 #endif
92
main(void)93 int main(void)
94 {
95 int cnt = 0;
96 char out_str[64];
97 struct sensor_value odr_attr;
98 const struct device *const lsm6dsl_dev = DEVICE_DT_GET_ONE(st_lsm6dsl);
99
100 if (!device_is_ready(lsm6dsl_dev)) {
101 printk("sensor: device not ready.\n");
102 return 0;
103 }
104
105 /* set accel/gyro sampling frequency to 104 Hz */
106 odr_attr.val1 = 104;
107 odr_attr.val2 = 0;
108
109 if (sensor_attr_set(lsm6dsl_dev, SENSOR_CHAN_ACCEL_XYZ,
110 SENSOR_ATTR_SAMPLING_FREQUENCY, &odr_attr) < 0) {
111 printk("Cannot set sampling frequency for accelerometer.\n");
112 return 0;
113 }
114
115 if (sensor_attr_set(lsm6dsl_dev, SENSOR_CHAN_GYRO_XYZ,
116 SENSOR_ATTR_SAMPLING_FREQUENCY, &odr_attr) < 0) {
117 printk("Cannot set sampling frequency for gyro.\n");
118 return 0;
119 }
120
121 #ifdef CONFIG_LSM6DSL_TRIGGER
122 struct sensor_trigger trig;
123
124 trig.type = SENSOR_TRIG_DATA_READY;
125 trig.chan = SENSOR_CHAN_ACCEL_XYZ;
126
127 if (sensor_trigger_set(lsm6dsl_dev, &trig, lsm6dsl_trigger_handler) != 0) {
128 printk("Could not set sensor type and channel\n");
129 return 0;
130 }
131 #endif
132
133 if (sensor_sample_fetch(lsm6dsl_dev) < 0) {
134 printk("Sensor sample update error\n");
135 return 0;
136 }
137
138 while (1) {
139 /* Erase previous */
140 printk("\0033\014");
141 printf("LSM6DSL sensor samples:\n\n");
142
143 /* lsm6dsl accel */
144 sprintf(out_str, "accel x:%f ms/2 y:%f ms/2 z:%f ms/2",
145 sensor_value_to_double(&accel_x_out),
146 sensor_value_to_double(&accel_y_out),
147 sensor_value_to_double(&accel_z_out));
148 printk("%s\n", out_str);
149
150 /* lsm6dsl gyro */
151 sprintf(out_str, "gyro x:%f dps y:%f dps z:%f dps",
152 sensor_value_to_double(&gyro_x_out),
153 sensor_value_to_double(&gyro_y_out),
154 sensor_value_to_double(&gyro_z_out));
155 printk("%s\n", out_str);
156
157 #if defined(CONFIG_LSM6DSL_EXT0_LIS2MDL)
158 /* lsm6dsl external magn */
159 sprintf(out_str, "magn x:%f gauss y:%f gauss z:%f gauss",
160 sensor_value_to_double(&magn_x_out),
161 sensor_value_to_double(&magn_y_out),
162 sensor_value_to_double(&magn_z_out));
163 printk("%s\n", out_str);
164 #endif
165
166 #if defined(CONFIG_LSM6DSL_EXT0_LPS22HB)
167 /* lsm6dsl external press/temp */
168 sprintf(out_str, "press: %f kPa - temp: %f deg",
169 sensor_value_to_double(&press_out), sensor_value_to_double(&temp_out));
170 printk("%s\n", out_str);
171 #endif
172
173 printk("loop:%d trig_cnt:%d\n\n", ++cnt, lsm6dsl_trig_cnt);
174
175 print_samples = 1;
176 k_sleep(K_MSEC(2000));
177 }
178 }
179
