1 /*
2 * Copyright (c) 2023 Intel Corporation.
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <stdlib.h>
8 #include <zephyr/drivers/sensor.h>
9 #include <zephyr/logging/log.h>
10 #include <zephyr/sys/util.h>
11 #include <zephyr/sensing/sensing_sensor.h>
12
13 LOG_MODULE_REGISTER(hinge_angle, CONFIG_SENSING_LOG_LEVEL);
14
15 #define HINGE_REPORTER_NUM 2
16
17 static struct sensing_sensor_register_info hinge_reg = {
18 .flags = SENSING_SENSOR_FLAG_REPORT_ON_CHANGE,
19 .sample_size = sizeof(struct sensing_sensor_value_q31),
20 .sensitivity_count = 1,
21 .version.value = SENSING_SENSOR_VERSION(1, 0, 0, 0),
22 };
23
24 struct hinge_angle_context {
25 struct rtio_iodev_sqe *sqe;
26 sensing_sensor_handle_t reporters[HINGE_REPORTER_NUM];
27 struct sensing_sensor_value_3d_q31 sample[HINGE_REPORTER_NUM];
28 int has_sample[HINGE_REPORTER_NUM];
29 };
30
hinge_init(const struct device * dev)31 static int hinge_init(const struct device *dev)
32 {
33 struct hinge_angle_context *data = dev->data;
34 int ret;
35
36 ret = sensing_sensor_get_reporters(dev,
37 SENSING_SENSOR_TYPE_MOTION_ACCELEROMETER_3D,
38 data->reporters, HINGE_REPORTER_NUM);
39 if (ret != HINGE_REPORTER_NUM) {
40 LOG_ERR("%s: reporter mismatch:%d", dev->name, ret);
41 return -ENODEV;
42 }
43
44 LOG_INF("%s:Found reporter 0: %s", dev->name,
45 sensing_get_sensor_info(data->reporters[0])->name);
46 LOG_INF("%s:Found reporter 1: %s", dev->name,
47 sensing_get_sensor_info(data->reporters[1])->name);
48
49 return 0;
50 }
51
hinge_attr_set(const struct device * dev,enum sensor_channel chan,enum sensor_attribute attr,const struct sensor_value * val)52 static int hinge_attr_set(const struct device *dev,
53 enum sensor_channel chan,
54 enum sensor_attribute attr,
55 const struct sensor_value *val)
56 {
57 struct sensing_sensor_config config = {0};
58 struct hinge_angle_context *data = dev->data;
59 int ret = 0;
60
61 switch (attr) {
62 case SENSOR_ATTR_SAMPLING_FREQUENCY:
63 config.attri = SENSING_SENSOR_ATTRIBUTE_INTERVAL;
64 config.interval = (uint32_t)(USEC_PER_SEC * 1000LL /
65 sensor_value_to_milli(val));
66 ret = sensing_set_config(data->reporters[0], &config, 1);
67 ret |= sensing_set_config(data->reporters[1], &config, 1);
68 break;
69
70 case SENSOR_ATTR_HYSTERESIS:
71 break;
72
73 default:
74 ret = -ENOTSUP;
75 break;
76 }
77
78 LOG_INF("%s set attr:%d ret:%d", dev->name, attr, ret);
79 return ret;
80 }
81
hinge_submit(const struct device * dev,struct rtio_iodev_sqe * sqe)82 static int hinge_submit(const struct device *dev,
83 struct rtio_iodev_sqe *sqe)
84 {
85 struct hinge_angle_context *data = dev->data;
86
87 if (data->sqe) {
88 return -EBUSY;
89 }
90
91 data->sqe = sqe;
92 return 0;
93 }
94
95 static const struct sensor_driver_api hinge_api = {
96 .attr_set = hinge_attr_set,
97 .submit = hinge_submit,
98 };
99
calc_hinge_angle(struct hinge_angle_context * data)100 static q31_t calc_hinge_angle(struct hinge_angle_context *data)
101 {
102 q31_t val;
103
104 LOG_INF("Acc 0: x:%08x y:%08x z:%08x",
105 data->sample[0].readings[0].x,
106 data->sample[0].readings[0].y,
107 data->sample[0].readings[0].z);
108 LOG_INF("Acc 1: x:%08x y:%08x z:%08x",
109 data->sample[1].readings[0].x,
110 data->sample[1].readings[0].y,
111 data->sample[1].readings[0].z);
112
113 /* Todo: calc hinge angle base on data->sample[0] and data->sample[1] */
114 val = 0;
115
116 return val;
117 }
118
hinge_reporter_on_data_event(sensing_sensor_handle_t handle,const void * buf,void * context)119 static void hinge_reporter_on_data_event(sensing_sensor_handle_t handle,
120 const void *buf, void *context)
121 {
122 struct hinge_angle_context *data = context;
123 struct sensing_sensor_value_q31 *sample;
124 uint32_t buffer_len = 0;
125 int both = 0;
126 int i, ret;
127
128 for (i = 0; i < HINGE_REPORTER_NUM; ++i) {
129 if (handle == data->reporters[i]) {
130 memcpy(&data->sample[i], buf, sizeof(data->sample[i]));
131 data->has_sample[i] = 1;
132 }
133 both += data->has_sample[i];
134 }
135
136 if (both == HINGE_REPORTER_NUM) {
137 data->has_sample[0] = 0;
138 data->has_sample[1] = 0;
139
140 ret = rtio_sqe_rx_buf(data->sqe, sizeof(*sample), sizeof(*sample),
141 (uint8_t **)&sample, &buffer_len);
142 if (ret) {
143 rtio_iodev_sqe_err(data->sqe, ret);
144 return;
145 }
146
147 sample->readings[0].v = calc_hinge_angle(data);
148
149 rtio_iodev_sqe_ok(data->sqe, 0);
150 }
151 }
152
153 #define DT_DRV_COMPAT zephyr_sensing_hinge_angle
154 #define SENSING_HINGE_ANGLE_DT_DEFINE(_inst) \
155 static struct hinge_angle_context _CONCAT(hinge_ctx, _inst); \
156 static struct sensing_callback_list _CONCAT(hinge_cb, _inst) = { \
157 .on_data_event = hinge_reporter_on_data_event, \
158 .context = &_CONCAT(hinge_ctx, _inst), \
159 }; \
160 SENSING_SENSORS_DT_INST_DEFINE(_inst, &hinge_reg, \
161 &_CONCAT(hinge_cb, _inst), \
162 &hinge_init, NULL, \
163 &_CONCAT(hinge_ctx, _inst), NULL, \
164 POST_KERNEL, CONFIG_SENSOR_INIT_PRIORITY, \
165 &hinge_api);
166
167 DT_INST_FOREACH_STATUS_OKAY(SENSING_HINGE_ANGLE_DT_DEFINE);
168
