1 /*
2 * Copyright (c) 2024 Analog Devices Inc.
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include "adxl345.h"
8
9 /** The q-scale factor will always be the same, as the nominal LSB/g
10 * changes at the same rate the selected shift parameter per range:
11 *
12 * - At 2G: 256 LSB/g, 10-bits resolution.
13 * - At 4g: 128 LSB/g, 10-bits resolution.
14 * - At 8g: 64 LSB/g, 10-bits resolution.
15 * - At 16g 32 LSB/g, 10-bits resolution.
16 */
17 static const uint32_t qscale_factor_no_full_res[] = {
18 /* (1.0 / Resolution-LSB-per-g * (2^31 / 2^5) * SENSOR_G / 1000000 */
19 [ADXL345_RANGE_2G] = UINT32_C(2570754),
20 /* (1.0 / Resolution-LSB-per-g) * (2^31 / 2^6) * SENSOR_G / 1000000 */
21 [ADXL345_RANGE_4G] = UINT32_C(2570754),
22 /* (1.0 / Resolution-LSB-per-g) * (2^31 / 2^7) ) * SENSOR_G / 1000000 */
23 [ADXL345_RANGE_8G] = UINT32_C(2570754),
24 /* (1.0 / Resolution-LSB-per-g) * (2^31 / 2^8) ) * SENSOR_G / 1000000 */
25 [ADXL345_RANGE_16G] = UINT32_C(2570754),
26 };
27
28
29 /** Sensitivities based on Range:
30 *
31 * - At 2G: 256 LSB/g, 10-bits resolution.
32 * - At 4g: 256 LSB/g, 11-bits resolution.
33 * - At 8g: 256 LSB/g, 12-bits resolution.
34 * - At 16g 256 LSB/g, 13-bits resolution.
35 */
36 static const uint32_t qscale_factor_full_res[] = {
37 /* (1.0 / Resolution-LSB-per-g) * (2^31 / 2^5) * SENSOR_G / 1000000 */
38 [ADXL345_RANGE_2G] = UINT32_C(2570754),
39 /* (1.0 / Resolution-LSB-per-g) * (2^31 / 2^6) * SENSOR_G / 1000000 */
40 [ADXL345_RANGE_4G] = UINT32_C(1285377),
41 /* (1.0 / Resolution-LSB-per-g) * (2^31 / 2^7) ) * SENSOR_G / 1000000 */
42 [ADXL345_RANGE_8G] = UINT32_C(642688),
43 /* (1.0 / Resolution-LSB-per-g) * (2^31 / 2^8) ) * SENSOR_G / 1000000 */
44 [ADXL345_RANGE_16G] = UINT32_C(321344),
45 };
46
47 static const uint32_t range_to_shift[] = {
48 [ADXL345_RANGE_2G] = 5,
49 [ADXL345_RANGE_4G] = 6,
50 [ADXL345_RANGE_8G] = 7,
51 [ADXL345_RANGE_16G] = 8,
52 };
53
adxl345_accel_convert_q31(q31_t * out,int16_t sample,int32_t range,uint8_t is_full_res)54 static inline void adxl345_accel_convert_q31(q31_t *out, int16_t sample, int32_t range,
55 uint8_t is_full_res)
56 {
57 if (is_full_res) {
58 switch (range) {
59 case ADXL345_RANGE_2G:
60 if (sample & BIT(9)) {
61 sample |= ADXL345_COMPLEMENT_MASK(10);
62 }
63 break;
64 case ADXL345_RANGE_4G:
65 if (sample & BIT(10)) {
66 sample |= ADXL345_COMPLEMENT_MASK(11);
67 }
68 break;
69 case ADXL345_RANGE_8G:
70 if (sample & BIT(11)) {
71 sample |= ADXL345_COMPLEMENT_MASK(12);
72 }
73 break;
74 case ADXL345_RANGE_16G:
75 if (sample & BIT(12)) {
76 sample |= ADXL345_COMPLEMENT_MASK(13);
77 }
78 break;
79 }
80 *out = sample * qscale_factor_full_res[range];
81 } else {
82 if (sample & BIT(9)) {
83 sample |= ADXL345_COMPLEMENT;
84 }
85 *out = sample * qscale_factor_no_full_res[range];
86 }
87 }
88
89 #ifdef CONFIG_ADXL345_STREAM
90
91 #define SENSOR_SCALING_FACTOR (SENSOR_G / (16 * 1000 / 100))
92
93 static const uint32_t accel_period_ns[] = {
94 [ADXL345_ODR_12HZ] = UINT32_C(1000000000) / 12,
95 [ADXL345_ODR_25HZ] = UINT32_C(1000000000) / 25,
96 [ADXL345_ODR_50HZ] = UINT32_C(1000000000) / 50,
97 [ADXL345_ODR_100HZ] = UINT32_C(1000000000) / 100,
98 [ADXL345_ODR_200HZ] = UINT32_C(1000000000) / 200,
99 [ADXL345_ODR_400HZ] = UINT32_C(1000000000) / 400,
100 };
101
adxl345_decode_stream(const uint8_t * buffer,struct sensor_chan_spec chan_spec,uint32_t * fit,uint16_t max_count,void * data_out)102 static int adxl345_decode_stream(const uint8_t *buffer, struct sensor_chan_spec chan_spec,
103 uint32_t *fit, uint16_t max_count, void *data_out)
104 {
105 const struct adxl345_fifo_data *enc_data = (const struct adxl345_fifo_data *)buffer;
106 const uint8_t *buffer_end =
107 buffer + sizeof(struct adxl345_fifo_data) + enc_data->fifo_byte_count;
108 int count = 0;
109 uint8_t sample_num = 0;
110
111 if ((uintptr_t)buffer_end <= *fit || chan_spec.chan_idx != 0) {
112 return 0;
113 }
114
115 struct sensor_three_axis_data *data = (struct sensor_three_axis_data *)data_out;
116
117 memset(data, 0, sizeof(struct sensor_three_axis_data));
118 data->header.base_timestamp_ns = enc_data->timestamp;
119 data->header.reading_count = 1;
120 data->shift = range_to_shift[enc_data->selected_range];
121
122 buffer += sizeof(struct adxl345_fifo_data);
123
124 uint8_t sample_set_size = enc_data->sample_set_size;
125 uint64_t period_ns = accel_period_ns[enc_data->accel_odr];
126 uint8_t is_full_res = enc_data->is_full_res;
127
128 /* Calculate which sample is decoded. */
129 if ((uint8_t *)*fit >= buffer) {
130 sample_num = ((uint8_t *)*fit - buffer) / sample_set_size;
131 }
132
133 while (count < max_count && buffer < buffer_end) {
134 const uint8_t *sample_end = buffer;
135
136 sample_end += sample_set_size;
137
138 if ((uintptr_t)buffer < *fit) {
139 /* This frame was already decoded, move on to the next frame */
140 buffer = sample_end;
141 continue;
142 }
143
144 switch (chan_spec.chan_type) {
145 case SENSOR_CHAN_ACCEL_XYZ:
146 data->readings[count].timestamp_delta = sample_num * period_ns;
147 uint8_t buff_offset = 0;
148
149 adxl345_accel_convert_q31(&data->readings[count].x, *(int16_t *)buffer,
150 enc_data->selected_range, is_full_res);
151 buff_offset = 2;
152 adxl345_accel_convert_q31(&data->readings[count].y,
153 *(int16_t *)(buffer + buff_offset),
154 enc_data->selected_range, is_full_res);
155 buff_offset += 2;
156 adxl345_accel_convert_q31(&data->readings[count].z,
157 *(int16_t *)(buffer + buff_offset),
158 enc_data->selected_range, is_full_res);
159 break;
160 default:
161 return -ENOTSUP;
162 }
163 buffer = sample_end;
164 *fit = (uintptr_t)sample_end;
165 count++;
166 }
167 return count;
168 }
169
170 #endif /* CONFIG_ADXL345_STREAM */
171
adxl345_decoder_get_frame_count(const uint8_t * buffer,struct sensor_chan_spec chan_spec,uint16_t * frame_count)172 static int adxl345_decoder_get_frame_count(const uint8_t *buffer, struct sensor_chan_spec chan_spec,
173 uint16_t *frame_count)
174 {
175 int32_t ret = -ENOTSUP;
176
177 if (chan_spec.chan_idx != 0) {
178 return ret;
179 }
180
181 #ifdef CONFIG_ADXL345_STREAM
182 const struct adxl345_fifo_data *data = (const struct adxl345_fifo_data *)buffer;
183
184 if (!data->is_fifo) {
185 #endif /* CONFIG_ADXL345_STREAM */
186 switch (chan_spec.chan_type) {
187 case SENSOR_CHAN_ACCEL_X:
188 case SENSOR_CHAN_ACCEL_Y:
189 case SENSOR_CHAN_ACCEL_Z:
190 case SENSOR_CHAN_ACCEL_XYZ:
191 *frame_count = 1;
192 ret = 0;
193 break;
194
195 default:
196 break;
197 }
198 #ifdef CONFIG_ADXL345_STREAM
199 } else {
200 if (data->fifo_byte_count == 0) {
201 *frame_count = 0;
202 ret = 0;
203 } else {
204 switch (chan_spec.chan_type) {
205 case SENSOR_CHAN_ACCEL_XYZ:
206 *frame_count =
207 data->fifo_byte_count / data->sample_set_size;
208 ret = 0;
209 break;
210
211 default:
212 break;
213 }
214 }
215 }
216 #endif /* CONFIG_ADXL345_STREAM */
217
218 return ret;
219 }
220
adxl345_decode_sample(const struct adxl345_sample * data,struct sensor_chan_spec chan_spec,uint32_t * fit,uint16_t max_count,void * data_out)221 static int adxl345_decode_sample(const struct adxl345_sample *data,
222 struct sensor_chan_spec chan_spec, uint32_t *fit,
223 uint16_t max_count, void *data_out)
224 {
225 struct sensor_three_axis_data *out = (struct sensor_three_axis_data *)data_out;
226
227 memset(out, 0, sizeof(struct sensor_three_axis_data));
228 out->header.base_timestamp_ns = k_ticks_to_ns_floor64(k_uptime_ticks());
229 out->header.reading_count = 1;
230 out->shift = range_to_shift[data->selected_range];
231
232 if (*fit > 0) {
233 return -ENOTSUP;
234 }
235
236 switch (chan_spec.chan_type) {
237 case SENSOR_CHAN_ACCEL_XYZ:
238 adxl345_accel_convert_q31(&out->readings->x, data->x, data->selected_range,
239 data->is_full_res);
240 adxl345_accel_convert_q31(&out->readings->y, data->y, data->selected_range,
241 data->is_full_res);
242 adxl345_accel_convert_q31(&out->readings->z, data->z, data->selected_range,
243 data->is_full_res);
244 break;
245 default:
246 return -ENOTSUP;
247 }
248
249 *fit = 1;
250
251 return 1;
252 }
253
adxl345_decoder_decode(const uint8_t * buffer,struct sensor_chan_spec chan_spec,uint32_t * fit,uint16_t max_count,void * data_out)254 static int adxl345_decoder_decode(const uint8_t *buffer, struct sensor_chan_spec chan_spec,
255 uint32_t *fit, uint16_t max_count, void *data_out)
256 {
257 const struct adxl345_sample *data = (const struct adxl345_sample *)buffer;
258
259 #ifdef CONFIG_ADXL345_STREAM
260 if (data->is_fifo) {
261 return adxl345_decode_stream(buffer, chan_spec, fit, max_count, data_out);
262 }
263 #endif /* CONFIG_ADXL345_STREAM */
264
265 return adxl345_decode_sample(data, chan_spec, fit, max_count, data_out);
266 }
267
adxl345_decoder_has_trigger(const uint8_t * buffer,enum sensor_trigger_type trigger)268 static bool adxl345_decoder_has_trigger(const uint8_t *buffer, enum sensor_trigger_type trigger)
269 {
270 const struct adxl345_fifo_data *data = (const struct adxl345_fifo_data *)buffer;
271
272 if (!data->is_fifo) {
273 return false;
274 }
275
276 switch (trigger) {
277 case SENSOR_TRIG_FIFO_WATERMARK:
278 return FIELD_GET(ADXL345_INT_MAP_WATERMARK_MSK, data->int_status);
279 default:
280 return false;
281 }
282 }
283
adxl345_get_size_info(struct sensor_chan_spec channel,size_t * base_size,size_t * frame_size)284 static int adxl345_get_size_info(struct sensor_chan_spec channel, size_t *base_size,
285 size_t *frame_size)
286 {
287 __ASSERT_NO_MSG(base_size != NULL);
288 __ASSERT_NO_MSG(frame_size != NULL);
289
290 if (channel.chan_type >= SENSOR_CHAN_ALL) {
291 return -ENOTSUP;
292 }
293
294 switch (channel.chan_type) {
295 case SENSOR_CHAN_ACCEL_XYZ:
296 *base_size = sizeof(struct sensor_three_axis_data);
297 *frame_size = sizeof(struct sensor_three_axis_sample_data);
298 return 0;
299 default:
300 break;
301 }
302
303 return -ENOTSUP;
304 }
305
306 SENSOR_DECODER_API_DT_DEFINE() = {
307 .get_frame_count = adxl345_decoder_get_frame_count,
308 .decode = adxl345_decoder_decode,
309 .has_trigger = adxl345_decoder_has_trigger,
310 .get_size_info = adxl345_get_size_info,
311 };
312
adxl345_get_decoder(const struct device * dev,const struct sensor_decoder_api ** decoder)313 int adxl345_get_decoder(const struct device *dev, const struct sensor_decoder_api **decoder)
314 {
315 ARG_UNUSED(dev);
316 *decoder = &SENSOR_DECODER_NAME();
317
318 return 0;
319 }
320
