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 void 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 rtio_iodev_sqe_err(sqe, -EBUSY);
89 } else {
90 data->sqe = sqe;
91 }
92 }
93
94 static const struct sensor_driver_api hinge_api = {
95 .attr_set = hinge_attr_set,
96 .submit = hinge_submit,
97 };
98
calc_hinge_angle(struct hinge_angle_context * data)99 static q31_t calc_hinge_angle(struct hinge_angle_context *data)
100 {
101 q31_t val;
102
103 LOG_INF("Acc 0: x:%08x y:%08x z:%08x",
104 data->sample[0].readings[0].x,
105 data->sample[0].readings[0].y,
106 data->sample[0].readings[0].z);
107 LOG_INF("Acc 1: x:%08x y:%08x z:%08x",
108 data->sample[1].readings[0].x,
109 data->sample[1].readings[0].y,
110 data->sample[1].readings[0].z);
111
112 /* Todo: calc hinge angle base on data->sample[0] and data->sample[1] */
113 val = 0;
114
115 return val;
116 }
117
hinge_reporter_on_data_event(sensing_sensor_handle_t handle,const void * buf,void * context)118 static void hinge_reporter_on_data_event(sensing_sensor_handle_t handle,
119 const void *buf, void *context)
120 {
121 struct hinge_angle_context *data = context;
122 struct sensing_sensor_value_q31 *sample;
123 uint32_t buffer_len = 0;
124 int both = 0;
125 int i, ret;
126
127 for (i = 0; i < HINGE_REPORTER_NUM; ++i) {
128 if (handle == data->reporters[i]) {
129 memcpy(&data->sample[i], buf, sizeof(data->sample[i]));
130 data->has_sample[i] = 1;
131 }
132 both += data->has_sample[i];
133 }
134
135 if (both == HINGE_REPORTER_NUM) {
136 data->has_sample[0] = 0;
137 data->has_sample[1] = 0;
138
139 ret = rtio_sqe_rx_buf(data->sqe, sizeof(*sample), sizeof(*sample),
140 (uint8_t **)&sample, &buffer_len);
141 if (ret) {
142 rtio_iodev_sqe_err(data->sqe, ret);
143 return;
144 }
145
146 sample->readings[0].v = calc_hinge_angle(data);
147
148 struct rtio_iodev_sqe *sqe = data->sqe;
149
150 data->sqe = NULL;
151 rtio_iodev_sqe_ok(sqe, 0);
152 }
153 }
154
155 #define DT_DRV_COMPAT zephyr_sensing_hinge_angle
156 #define SENSING_HINGE_ANGLE_DT_DEFINE(_inst) \
157 static struct hinge_angle_context _CONCAT(hinge_ctx, _inst); \
158 static struct sensing_callback_list _CONCAT(hinge_cb, _inst) = { \
159 .on_data_event = hinge_reporter_on_data_event, \
160 .context = &_CONCAT(hinge_ctx, _inst), \
161 }; \
162 SENSING_SENSORS_DT_INST_DEFINE(_inst, &hinge_reg, \
163 &_CONCAT(hinge_cb, _inst), \
164 &hinge_init, NULL, \
165 &_CONCAT(hinge_ctx, _inst), NULL, \
166 POST_KERNEL, CONFIG_SENSOR_INIT_PRIORITY, \
167 &hinge_api);
168
169 DT_INST_FOREACH_STATUS_OKAY(SENSING_HINGE_ANGLE_DT_DEFINE);
170
