1 /*
2 * Copyright (c) 2020 Yestin Sun
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <stdio.h>
8 #include <zephyr/kernel.h>
9 #include <zephyr/device.h>
10 #include <zephyr/drivers/sensor.h>
11
out_ev(struct sensor_value * val)12 static inline float out_ev(struct sensor_value *val)
13 {
14 return (val->val1 + (float)val->val2 / 1000000);
15 }
16
fetch_and_display(const struct device * dev)17 static void fetch_and_display(const struct device *dev)
18 {
19 struct sensor_value x, y, z;
20 static int trig_cnt;
21
22 trig_cnt++;
23
24 /* lsm6dso accel */
25 sensor_sample_fetch_chan(dev, SENSOR_CHAN_ACCEL_XYZ);
26 sensor_channel_get(dev, SENSOR_CHAN_ACCEL_X, &x);
27 sensor_channel_get(dev, SENSOR_CHAN_ACCEL_Y, &y);
28 sensor_channel_get(dev, SENSOR_CHAN_ACCEL_Z, &z);
29
30 printf("accel x:%f ms/2 y:%f ms/2 z:%f ms/2\n",
31 (double)out_ev(&x), (double)out_ev(&y), (double)out_ev(&z));
32
33 /* lsm6dso gyro */
34 sensor_sample_fetch_chan(dev, SENSOR_CHAN_GYRO_XYZ);
35 sensor_channel_get(dev, SENSOR_CHAN_GYRO_X, &x);
36 sensor_channel_get(dev, SENSOR_CHAN_GYRO_Y, &y);
37 sensor_channel_get(dev, SENSOR_CHAN_GYRO_Z, &z);
38
39 printf("gyro x:%f rad/s y:%f rad/s z:%f rad/s\n",
40 (double)out_ev(&x), (double)out_ev(&y), (double)out_ev(&z));
41
42 printf("trig_cnt:%d\n\n", trig_cnt);
43 }
44
set_sampling_freq(const struct device * dev)45 static int set_sampling_freq(const struct device *dev)
46 {
47 int ret = 0;
48 struct sensor_value odr_attr;
49
50 /* set accel/gyro sampling frequency to 12.5 Hz */
51 odr_attr.val1 = 12.5;
52 odr_attr.val2 = 0;
53
54 ret = sensor_attr_set(dev, SENSOR_CHAN_ACCEL_XYZ,
55 SENSOR_ATTR_SAMPLING_FREQUENCY, &odr_attr);
56 if (ret != 0) {
57 printf("Cannot set sampling frequency for accelerometer.\n");
58 return ret;
59 }
60
61 ret = sensor_attr_set(dev, SENSOR_CHAN_GYRO_XYZ,
62 SENSOR_ATTR_SAMPLING_FREQUENCY, &odr_attr);
63 if (ret != 0) {
64 printf("Cannot set sampling frequency for gyro.\n");
65 return ret;
66 }
67
68 return 0;
69 }
70
71 #ifdef CONFIG_LSM6DSO_TRIGGER
trigger_handler(const struct device * dev,const struct sensor_trigger * trig)72 static void trigger_handler(const struct device *dev,
73 const struct sensor_trigger *trig)
74 {
75 fetch_and_display(dev);
76 }
77
test_trigger_mode(const struct device * dev)78 static void test_trigger_mode(const struct device *dev)
79 {
80 struct sensor_trigger trig;
81
82 if (set_sampling_freq(dev) != 0) {
83 return;
84 }
85
86 trig.type = SENSOR_TRIG_DATA_READY;
87 trig.chan = SENSOR_CHAN_ACCEL_XYZ;
88
89 if (sensor_trigger_set(dev, &trig, trigger_handler) != 0) {
90 printf("Could not set sensor type and channel\n");
91 return;
92 }
93 }
94
95 #else
test_polling_mode(const struct device * dev)96 static void test_polling_mode(const struct device *dev)
97 {
98 if (set_sampling_freq(dev) != 0) {
99 return;
100 }
101
102 while (1) {
103 fetch_and_display(dev);
104 k_sleep(K_MSEC(1000));
105 }
106 }
107 #endif
108
main(void)109 int main(void)
110 {
111 const struct device *const dev = DEVICE_DT_GET_ONE(st_lsm6dso);
112
113 if (!device_is_ready(dev)) {
114 printk("%s: device not ready.\n", dev->name);
115 return 0;
116 }
117
118 #ifdef CONFIG_LSM6DSO_TRIGGER
119 printf("Testing LSM6DSO sensor in trigger mode.\n\n");
120 test_trigger_mode(dev);
121 #else
122 printf("Testing LSM6DSO sensor in polling mode.\n\n");
123 test_polling_mode(dev);
124 #endif
125 return 0;
126 }
127
