1 /*
2 * Copyright (c) 2022 TOKITA Hiroshi <tokita.hiroshi@fujitsu.com
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <stdio.h>
8 #include <stdlib.h>
9
10 #include <zephyr/device.h>
11 #include <zephyr/drivers/sensor.h>
12 #include <zephyr/sys/util_macro.h>
13 #include <zephyr/kernel.h>
14 #include <zephyr/rtio/rtio.h>
15
16 #define ACCEL_ALIAS(i) DT_ALIAS(_CONCAT(accel, i))
17 #define ACCELEROMETER_DEVICE(i, _) \
18 IF_ENABLED(DT_NODE_EXISTS(ACCEL_ALIAS(i)), (DEVICE_DT_GET(ACCEL_ALIAS(i)),))
19 #define NUM_SENSORS 1
20
21 /* support up to 10 accelerometer sensors */
22 static const struct device *const sensors[] = {LISTIFY(10, ACCELEROMETER_DEVICE, ())};
23
24 #ifdef CONFIG_SENSOR_ASYNC_API
25
26 #define ACCEL_IODEV_SYM(id) CONCAT(accel_iodev, id)
27 #define ACCEL_IODEV_PTR(id, _) &ACCEL_IODEV_SYM(id)
28
29 #define ACCEL_TRIGGERS \
30 {SENSOR_TRIG_FIFO_FULL, SENSOR_STREAM_DATA_INCLUDE}, \
31 {SENSOR_TRIG_FIFO_WATERMARK, SENSOR_STREAM_DATA_INCLUDE}
32
33 #define ACCEL_DEFINE_IODEV(id, _) \
34 SENSOR_DT_STREAM_IODEV( \
35 ACCEL_IODEV_SYM(id), \
36 ACCEL_ALIAS(id), \
37 ACCEL_TRIGGERS);
38
39 LISTIFY(NUM_SENSORS, ACCEL_DEFINE_IODEV, (;));
40
41 struct rtio_iodev *iodevs[NUM_SENSORS] = { LISTIFY(NUM_SENSORS, ACCEL_IODEV_PTR, (,)) };
42
43 RTIO_DEFINE_WITH_MEMPOOL(accel_ctx, NUM_SENSORS, NUM_SENSORS, NUM_SENSORS*20, 256, sizeof(void *));
44
print_accels_stream(const struct device * dev,struct rtio_iodev * iodev)45 static int print_accels_stream(const struct device *dev, struct rtio_iodev *iodev)
46 {
47 int rc = 0;
48 struct sensor_three_axis_data accel_data = {0};
49 const struct sensor_decoder_api *decoder;
50 struct rtio_cqe *cqe;
51 uint8_t *buf;
52 uint32_t buf_len;
53 struct rtio_sqe *handles[NUM_SENSORS];
54
55 /* Start the streams */
56 for (int i = 0; i < NUM_SENSORS; i++) {
57 printk("sensor_stream\n");
58 sensor_stream(iodevs[i], &accel_ctx, NULL, &handles[i]);
59 }
60
61 while (1) {
62 cqe = rtio_cqe_consume_block(&accel_ctx);
63
64 if (cqe->result != 0) {
65 printk("async read failed %d\n", cqe->result);
66 return cqe->result;
67 }
68
69 rc = rtio_cqe_get_mempool_buffer(&accel_ctx, cqe, &buf, &buf_len);
70
71 if (rc != 0) {
72 printk("get mempool buffer failed %d\n", rc);
73 return rc;
74 }
75
76 const struct device *sensor = dev;
77
78 rtio_cqe_release(&accel_ctx, cqe);
79
80 rc = sensor_get_decoder(sensor, &decoder);
81
82 if (rc != 0) {
83 printk("sensor_get_decoder failed %d\n", rc);
84 return rc;
85 }
86
87 /* Frame iterator values when data comes from a FIFO */
88 uint32_t accel_fit = 0;
89
90 /* Number of accelerometer data frames */
91 uint16_t frame_count;
92
93 rc = decoder->get_frame_count(buf,
94 (struct sensor_chan_spec) {SENSOR_CHAN_ACCEL_XYZ, 0}, &frame_count);
95
96 if (rc != 0) {
97 printk("sensor_get_decoder failed %d\n", rc);
98 return rc;
99 }
100
101 /* If a tap has occurred lets print it out */
102 if (decoder->has_trigger(buf, SENSOR_TRIG_TAP)) {
103 printk("Tap! Sensor %s\n", dev->name);
104 }
105
106 /* Decode all available accelerometer sample frames */
107 for (int i = 0; i < frame_count; i++) {
108 decoder->decode(buf, (struct sensor_chan_spec) {SENSOR_CHAN_ACCEL_XYZ, 0},
109 &accel_fit, 1, &accel_data);
110
111 printk("Accel data for %s (%" PRIq(6) ", %" PRIq(6)
112 ", %" PRIq(6) ") %lluns\n", dev->name,
113 PRIq_arg(accel_data.readings[0].x, 6, accel_data.shift),
114 PRIq_arg(accel_data.readings[0].y, 6, accel_data.shift),
115 PRIq_arg(accel_data.readings[0].z, 6, accel_data.shift),
116 (accel_data.header.base_timestamp_ns
117 + accel_data.readings[0].timestamp_delta));
118 }
119
120 rtio_release_buffer(&accel_ctx, buf, buf_len);
121 }
122
123 return rc;
124 }
125 #else
126
127 static const enum sensor_channel channels[] = {
128 SENSOR_CHAN_ACCEL_X,
129 SENSOR_CHAN_ACCEL_Y,
130 SENSOR_CHAN_ACCEL_Z,
131 };
132
print_accels(const struct device * dev)133 static int print_accels(const struct device *dev)
134 {
135 int ret;
136 struct sensor_value accel[3];
137
138 ret = sensor_sample_fetch(dev);
139 if (ret < 0) {
140 printk("%s: sensor_sample_fetch() failed: %d\n", dev->name, ret);
141 return ret;
142 }
143
144 for (size_t i = 0; i < ARRAY_SIZE(channels); i++) {
145 ret = sensor_channel_get(dev, channels[i], &accel[i]);
146 if (ret < 0) {
147 printk("%s: sensor_channel_get(%c) failed: %d\n", dev->name, 'X' + i, ret);
148 return ret;
149 }
150 }
151
152 printk("%16s [m/s^2]: (%12.6f, %12.6f, %12.6f)\n", dev->name,
153 sensor_value_to_double(&accel[0]), sensor_value_to_double(&accel[1]),
154 sensor_value_to_double(&accel[2]));
155
156 return 0;
157 }
158 #endif /*CONFIG_SENSOR_ASYNC_API*/
159
set_sampling_freq(const struct device * dev)160 static int set_sampling_freq(const struct device *dev)
161 {
162 int ret;
163 struct sensor_value odr;
164
165 ret = sensor_attr_get(dev, SENSOR_CHAN_ACCEL_XYZ, SENSOR_ATTR_SAMPLING_FREQUENCY, &odr);
166
167 /* If we don't get a frequency > 0, we set one */
168 if (ret != 0 || (odr.val1 == 0 && odr.val2 == 0)) {
169 odr.val1 = 100;
170 odr.val2 = 0;
171
172 ret = sensor_attr_set(dev, SENSOR_CHAN_ACCEL_XYZ, SENSOR_ATTR_SAMPLING_FREQUENCY,
173 &odr);
174
175 if (ret != 0) {
176 printk("%s : failed to set sampling frequency\n", dev->name);
177 }
178 }
179
180 return 0;
181 }
182
main(void)183 int main(void)
184 {
185 int ret;
186
187 for (size_t i = 0; i < ARRAY_SIZE(sensors); i++) {
188 if (!device_is_ready(sensors[i])) {
189 printk("sensor: device %s not ready.\n", sensors[i]->name);
190 return 0;
191 }
192 set_sampling_freq(sensors[i]);
193 }
194
195 #ifndef CONFIG_COVERAGE
196 while (1) {
197 #else
198 for (int i = 0; i < 5; i++) {
199 #endif
200 for (size_t i = 0; i < ARRAY_SIZE(sensors); i++) {
201 #ifdef CONFIG_SENSOR_ASYNC_API
202 ret = print_accels_stream(sensors[i], iodevs[i]);
203 #else
204 ret = print_accels(sensors[i]);
205 #endif /*CONFIG_SENSOR_ASYNC_API*/
206 if (ret < 0) {
207 return 0;
208 }
209 }
210 k_msleep(1000);
211 }
212 return 0;
213 }
214
