1 /*
2 * Copyright (c) 2023 Intel Corporation.
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include "zephyr/devicetree.h"
8 #define DT_DRV_COMPAT zephyr_sensing_phy_3d_sensor
9
10 #include <stdlib.h>
11 #include <zephyr/drivers/sensor.h>
12 #include <zephyr/logging/log.h>
13 #include <zephyr/rtio/rtio.h>
14 #include <zephyr/sys_clock.h>
15 #include <zephyr/sensing/sensing.h>
16 #include <zephyr/sensing/sensing_sensor.h>
17 #include <zephyr/sys/util.h>
18
19 #include "phy_3d_sensor.h"
20
21 LOG_MODULE_REGISTER(phy_3d_sensor, CONFIG_SENSING_LOG_LEVEL);
22
23 #define SENSING_ACCEL_Q31_SHIFT 6
24 #define SENSING_GYRO_Q31_SHIFT 15
25
shifted_q31_to_scaled_int64(q31_t q,int8_t shift,int64_t scale)26 static int64_t shifted_q31_to_scaled_int64(q31_t q, int8_t shift, int64_t scale)
27 {
28 int64_t scaled_value;
29 int64_t shifted_value;
30
31 shifted_value = (int64_t)q << shift;
32 shifted_value = llabs(shifted_value);
33
34 scaled_value =
35 FIELD_GET(GENMASK64(31 + shift, 31), shifted_value) * scale +
36 (FIELD_GET(GENMASK64(30, 0), shifted_value) * scale / BIT(31));
37
38 if (q < 0) {
39 scaled_value = -scaled_value;
40 }
41
42 return scaled_value;
43 }
44
scaled_int64_to_shifted_q31(int64_t val,int64_t scale,int8_t shift)45 static q31_t scaled_int64_to_shifted_q31(int64_t val, int64_t scale,
46 int8_t shift)
47 {
48 return (q31_t)((val * BIT(31 - shift) / scale));
49 }
50
accel_sensor_value_to_q31(struct sensor_value * val)51 static q31_t accel_sensor_value_to_q31(struct sensor_value *val)
52 {
53 int64_t micro_ms2 = sensor_value_to_micro(val);
54 int64_t micro_g = micro_ms2 * 1000000LL / SENSOR_G;
55
56 return scaled_int64_to_shifted_q31(micro_g, 1000000LL,
57 SENSING_ACCEL_Q31_SHIFT);
58 }
59
accel_q31_to_sensor_value(q31_t q,struct sensor_value * val)60 static void accel_q31_to_sensor_value(q31_t q, struct sensor_value *val)
61 {
62 int64_t micro_g = shifted_q31_to_scaled_int64(q,
63 SENSING_ACCEL_Q31_SHIFT, 1000000LL);
64 int64_t micro_ms2 = micro_g * SENSOR_G / 1000000LL;
65
66 sensor_value_from_micro(val, micro_ms2);
67 }
68
69 static const struct phy_3d_sensor_custom custom_accel = {
70 .chan_all = SENSOR_CHAN_ACCEL_XYZ,
71 .shift = SENSING_ACCEL_Q31_SHIFT,
72 .q31_to_sensor_value = accel_q31_to_sensor_value,
73 .sensor_value_to_q31 = accel_sensor_value_to_q31,
74 };
75
gyro_sensor_value_to_q31(struct sensor_value * val)76 static q31_t gyro_sensor_value_to_q31(struct sensor_value *val)
77 {
78 int64_t micro_rad = (int64_t)sensor_value_to_micro(val);
79 int64_t micro_deg = micro_rad * 180000000LL / SENSOR_PI;
80
81 return scaled_int64_to_shifted_q31(micro_deg, 1000000LL,
82 SENSING_GYRO_Q31_SHIFT);
83 }
84
gyro_q31_to_sensor_value(q31_t q,struct sensor_value * val)85 static void gyro_q31_to_sensor_value(q31_t q, struct sensor_value *val)
86 {
87 int64_t micro_deg = shifted_q31_to_scaled_int64(q,
88 SENSING_GYRO_Q31_SHIFT, 1000000LL);
89 int64_t micro_rad = micro_deg * SENSOR_PI / 180000000LL;
90
91 sensor_value_from_micro(val, micro_rad);
92 }
93
94 static const struct phy_3d_sensor_custom custom_gyro = {
95 .chan_all = SENSOR_CHAN_GYRO_XYZ,
96 .shift = SENSING_GYRO_Q31_SHIFT,
97 .q31_to_sensor_value = gyro_q31_to_sensor_value,
98 .sensor_value_to_q31 = gyro_sensor_value_to_q31,
99 };
100
phy_3d_sensor_init(const struct device * dev)101 static int phy_3d_sensor_init(const struct device *dev)
102 {
103 const struct phy_3d_sensor_config *cfg = dev->config;
104 struct phy_3d_sensor_data *data = dev->data;
105 int i;
106
107 for (i = 0; i < cfg->sensor_num; i++) {
108 switch (cfg->sensor_types[i]) {
109 case SENSING_SENSOR_TYPE_MOTION_ACCELEROMETER_3D:
110 data->customs[i] = &custom_accel;
111 break;
112 case SENSING_SENSOR_TYPE_MOTION_GYROMETER_3D:
113 data->customs[i] = &custom_gyro;
114 break;
115 default:
116 LOG_ERR("phy_3d_sensor doesn't support sensor type %d",
117 cfg->sensor_types[i]);
118 return -ENOTSUP;
119 }
120 }
121
122 LOG_INF("%s: Underlying device: %s", dev->name, cfg->hw_dev->name);
123
124 return 0;
125 }
126
phy_3d_sensor_attr_set_hyst(const struct device * dev,enum sensor_channel chan,const struct sensor_value * val)127 static int phy_3d_sensor_attr_set_hyst(const struct device *dev,
128 enum sensor_channel chan,
129 const struct sensor_value *val)
130 {
131 const struct phy_3d_sensor_config *cfg = dev->config;
132
133 if (chan == SENSOR_CHAN_ACCEL_XYZ || chan == SENSOR_CHAN_GYRO_XYZ) {
134 return sensor_attr_set(cfg->hw_dev, chan, SENSOR_ATTR_SLOPE_TH, val);
135 } else {
136 return sensor_attr_set(cfg->hw_dev, chan, SENSOR_ATTR_HYSTERESIS, val);
137 }
138 }
139
phy_3d_sensor_attr_set(const struct device * dev,enum sensor_channel chan,enum sensor_attribute attr,const struct sensor_value * val)140 static int phy_3d_sensor_attr_set(const struct device *dev,
141 enum sensor_channel chan,
142 enum sensor_attribute attr,
143 const struct sensor_value *val)
144 {
145 const struct phy_3d_sensor_config *cfg = dev->config;
146 int ret = 0;
147
148 switch (attr) {
149 case SENSOR_ATTR_HYSTERESIS:
150 ret = phy_3d_sensor_attr_set_hyst(dev, chan, val);
151 break;
152
153 default:
154 ret = sensor_attr_set(cfg->hw_dev, chan, attr, val);
155 break;
156 }
157
158 LOG_INF("%s:%s attr:%d ret:%d", __func__, dev->name, attr, ret);
159 return ret;
160 }
161
phy_3d_sensor_submit(const struct device * dev,struct rtio_iodev_sqe * sqe)162 static void phy_3d_sensor_submit(const struct device *dev,
163 struct rtio_iodev_sqe *sqe)
164 {
165 struct sensing_submit_config *config = (struct sensing_submit_config *)sqe->sqe.iodev->data;
166 const struct phy_3d_sensor_config *cfg = dev->config;
167 struct phy_3d_sensor_data *data = dev->data;
168 const struct phy_3d_sensor_custom *custom = data->customs[config->info_index];
169 struct sensing_sensor_value_3d_q31 *sample;
170 struct sensor_value value[PHY_3D_SENSOR_CHANNEL_NUM];
171 uint32_t buffer_len;
172 int i, ret;
173
174 ret = rtio_sqe_rx_buf(sqe, sizeof(*sample), sizeof(*sample),
175 (uint8_t **)&sample, &buffer_len);
176 if (ret) {
177 rtio_iodev_sqe_err(sqe, ret);
178 return;
179 }
180
181 ret = sensor_sample_fetch_chan(cfg->hw_dev, custom->chan_all);
182 if (ret) {
183 LOG_ERR("%s: sample fetch failed: %d", dev->name, ret);
184 rtio_iodev_sqe_err(sqe, ret);
185 return;
186 }
187
188 ret = sensor_channel_get(cfg->hw_dev, custom->chan_all, value);
189 if (ret) {
190 LOG_ERR("%s: channel get failed: %d", dev->name, ret);
191 rtio_iodev_sqe_err(sqe, ret);
192 return;
193 }
194
195 for (i = 0; i < ARRAY_SIZE(value); ++i) {
196 sample->readings[0].v[i] = custom->sensor_value_to_q31(&value[i]);
197 }
198
199 sample->header.reading_count = 1;
200 sample->shift = custom->shift;
201
202 LOG_DBG("%s: Sample data:\t x: %d, y: %d, z: %d",
203 dev->name,
204 sample->readings[0].x,
205 sample->readings[0].y,
206 sample->readings[0].z);
207
208 rtio_iodev_sqe_ok(sqe, 0);
209 return;
210 }
211
212 static DEVICE_API(sensor, phy_3d_sensor_api) = {
213 .attr_set = phy_3d_sensor_attr_set,
214 .submit = phy_3d_sensor_submit,
215 };
216
217 /* To be removed */
218 static const struct sensing_sensor_register_info phy_3d_sensor_reg = {
219 .flags = SENSING_SENSOR_FLAG_REPORT_ON_CHANGE,
220 .sample_size = sizeof(struct sensing_sensor_value_3d_q31),
221 .sensitivity_count = PHY_3D_SENSOR_CHANNEL_NUM,
222 .version.value = SENSING_SENSOR_VERSION(0, 8, 0, 0),
223 };
224
225 #define SENSING_PHY_3D_SENSOR_DT_DEFINE(_inst) \
226 static struct rtio_iodev_sqe _CONCAT(sqes, _inst)[DT_INST_PROP_LEN(_inst, \
227 sensor_types)]; \
228 static const struct phy_3d_sensor_custom *_CONCAT(customs, _inst) \
229 [DT_INST_PROP_LEN(_inst, sensor_types)]; \
230 static struct phy_3d_sensor_data _CONCAT(data, _inst) = { \
231 .sqes = _CONCAT(sqes, _inst), \
232 .customs = _CONCAT(customs, _inst), \
233 }; \
234 static const struct phy_3d_sensor_config _CONCAT(cfg, _inst) = { \
235 .hw_dev = DEVICE_DT_GET( \
236 DT_PHANDLE(DT_DRV_INST(_inst), \
237 underlying_device)), \
238 .sensor_num = DT_INST_PROP_LEN(_inst, sensor_types), \
239 .sensor_types = DT_PROP(DT_DRV_INST(_inst), sensor_types), \
240 }; \
241 SENSING_SENSORS_DT_INST_DEFINE(_inst, &phy_3d_sensor_reg, NULL, \
242 &phy_3d_sensor_init, NULL, \
243 &_CONCAT(data, _inst), &_CONCAT(cfg, _inst), \
244 POST_KERNEL, CONFIG_SENSOR_INIT_PRIORITY, \
245 &phy_3d_sensor_api);
246
247 DT_INST_FOREACH_STATUS_OKAY(SENSING_PHY_3D_SENSOR_DT_DEFINE);
248