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