1 /*
2 * Copyright (c) 2022 Valerio Setti <valerio.setti@gmail.com>
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 <zephyr/sys/printk.h>
11
12 #define QUAD_ENC_EMUL_ENABLED \
13 DT_NODE_EXISTS(DT_ALIAS(qenca)) && DT_NODE_EXISTS(DT_ALIAS(qencb))
14
15 #if QUAD_ENC_EMUL_ENABLED
16
17 #include <zephyr/drivers/gpio.h>
18
19 #define QUAD_ENC_EMUL_PERIOD 100
20
21 static const struct gpio_dt_spec phase_a =
22 GPIO_DT_SPEC_GET(DT_ALIAS(qenca), gpios);
23 static const struct gpio_dt_spec phase_b =
24 GPIO_DT_SPEC_GET(DT_ALIAS(qencb), gpios);
25 static bool toggle_a;
26
qenc_emulate_work_handler(struct k_work * work)27 void qenc_emulate_work_handler(struct k_work *work)
28 {
29 toggle_a = !toggle_a;
30 if (toggle_a) {
31 gpio_pin_toggle_dt(&phase_a);
32 } else {
33 gpio_pin_toggle_dt(&phase_b);
34 }
35 }
36
37 static K_WORK_DEFINE(qenc_emulate_work, qenc_emulate_work_handler);
38
qenc_emulate_timer_handler(struct k_timer * dummy)39 static void qenc_emulate_timer_handler(struct k_timer *dummy)
40 {
41 k_work_submit(&qenc_emulate_work);
42 }
43
44 static K_TIMER_DEFINE(qenc_emulate_timer, qenc_emulate_timer_handler, NULL);
45
qenc_emulate_init(void)46 static void qenc_emulate_init(void)
47 {
48 printk("Quadrature encoder emulator enabled with %u ms period\n",
49 QUAD_ENC_EMUL_PERIOD);
50
51 if (!gpio_is_ready_dt(&phase_a)) {
52 printk("%s: device not ready.", phase_a.port->name);
53 return;
54 }
55 gpio_pin_configure_dt(&phase_a, GPIO_OUTPUT);
56
57 if (!gpio_is_ready_dt(&phase_b)) {
58 printk("%s: device not ready.", phase_b.port->name);
59 return;
60 }
61 gpio_pin_configure_dt(&phase_b, GPIO_OUTPUT);
62
63 k_timer_start(&qenc_emulate_timer, K_MSEC(QUAD_ENC_EMUL_PERIOD / 2),
64 K_MSEC(QUAD_ENC_EMUL_PERIOD / 2));
65 }
66
67 #else
68
qenc_emulate_init(void)69 static void qenc_emulate_init(void) { };
70
71 #endif /* QUAD_ENC_EMUL_ENABLED */
72
main(void)73 int main(void)
74 {
75 struct sensor_value val;
76 int rc;
77 const struct device *const dev = DEVICE_DT_GET(DT_ALIAS(qdec0));
78
79 if (!device_is_ready(dev)) {
80 printk("Qdec device is not ready\n");
81 return 0;
82 }
83
84 printk("Quadrature decoder sensor test\n");
85
86 qenc_emulate_init();
87
88 #ifndef CONFIG_COVERAGE
89 while (true) {
90 #else
91 for (int i = 0; i < 3; i++) {
92 #endif
93 /* sleep first to gather position from first period */
94 k_msleep(1000);
95
96 rc = sensor_sample_fetch(dev);
97 if (rc != 0) {
98 printk("Failed to fetch sample (%d)\n", rc);
99 return 0;
100 }
101
102 rc = sensor_channel_get(dev, SENSOR_CHAN_ROTATION, &val);
103 if (rc != 0) {
104 printk("Failed to get data (%d)\n", rc);
105 return 0;
106 }
107
108 printk("Position = %d degrees\n", val.val1);
109 }
110 return 0;
111 }
112
