1 /*
2 * Copyright (c) 2025 Croxel Inc.
3 * Copyright (c) 2025 CogniPilot Foundation
4 *
5 * SPDX-License-Identifier: Apache-2.0
6 */
7
8 #include <zephyr/drivers/sensor.h>
9 #include <zephyr/drivers/sensor_clock.h>
10 #include <zephyr/sys/util.h>
11 #include <zephyr/sys/byteorder.h>
12 #include <zephyr/dt-bindings/sensor/rm3100.h>
13 #include "rm3100.h"
14
rm3100_encode_channel(enum sensor_channel chan)15 uint8_t rm3100_encode_channel(enum sensor_channel chan)
16 {
17 switch (chan) {
18 case SENSOR_CHAN_MAGN_X:
19 return BIT(0);
20 case SENSOR_CHAN_MAGN_Y:
21 return BIT(1);
22 case SENSOR_CHAN_MAGN_Z:
23 return BIT(2);
24 case SENSOR_CHAN_ALL:
25 case SENSOR_CHAN_MAGN_XYZ:
26 return BIT(0) | BIT(1) | BIT(2);
27 default:
28 return 0;
29 }
30 }
31
rm3100_encode(const struct device * dev,const struct sensor_chan_spec * const channels,size_t num_channels,uint8_t * buf)32 int rm3100_encode(const struct device *dev,
33 const struct sensor_chan_spec *const channels,
34 size_t num_channels,
35 uint8_t *buf)
36 {
37 const struct rm3100_data *data = dev->data;
38 struct rm3100_encoded_data *edata = (struct rm3100_encoded_data *)buf;
39 uint64_t cycles;
40 int err;
41
42 edata->header.channels = 0;
43
44 if (data->settings.odr == RM3100_DT_ODR_600) {
45 edata->header.cycle_count = RM3100_CYCLE_COUNT_HIGH_ODR;
46 } else {
47 edata->header.cycle_count = RM3100_CYCLE_COUNT_DEFAULT;
48 }
49
50 for (size_t i = 0; i < num_channels; i++) {
51 edata->header.channels |= rm3100_encode_channel(channels[i].chan_type);
52 }
53
54 err = sensor_clock_get_cycles(&cycles);
55 if (err != 0) {
56 return err;
57 }
58
59 edata->header.timestamp = sensor_clock_cycles_to_ns(cycles);
60
61 return 0;
62 }
63
rm3100_decoder_get_size_info(struct sensor_chan_spec chan_spec,size_t * base_size,size_t * frame_size)64 static int rm3100_decoder_get_size_info(struct sensor_chan_spec chan_spec,
65 size_t *base_size,
66 size_t *frame_size)
67 {
68 switch (chan_spec.chan_type) {
69 case SENSOR_CHAN_MAGN_X:
70 case SENSOR_CHAN_MAGN_Y:
71 case SENSOR_CHAN_MAGN_Z:
72 *base_size = sizeof(struct sensor_q31_data);
73 *frame_size = sizeof(struct sensor_q31_sample_data);
74 return 0;
75 case SENSOR_CHAN_MAGN_XYZ:
76 *base_size = sizeof(struct sensor_three_axis_data);
77 *frame_size = sizeof(struct sensor_three_axis_data);
78 return 0;
79 default:
80 return -ENOTSUP;
81 }
82
83 return 0;
84 }
85
rm3100_decoder_get_frame_count(const uint8_t * buffer,struct sensor_chan_spec chan_spec,uint16_t * frame_count)86 static int rm3100_decoder_get_frame_count(const uint8_t *buffer,
87 struct sensor_chan_spec chan_spec,
88 uint16_t *frame_count)
89 {
90 struct rm3100_encoded_data *edata = (struct rm3100_encoded_data *)buffer;
91
92 if (chan_spec.chan_idx != 0) {
93 return -ENOTSUP;
94 }
95
96 uint8_t channel_request = rm3100_encode_channel(chan_spec.chan_type);
97
98 if (((edata->header.channels & channel_request) != channel_request)) {
99 return -ENODATA;
100 }
101
102 switch (chan_spec.chan_type) {
103 case SENSOR_CHAN_MAGN_X:
104 case SENSOR_CHAN_MAGN_Y:
105 case SENSOR_CHAN_MAGN_Z:
106 case SENSOR_CHAN_MAGN_XYZ:
107 *frame_count = 1;
108 return 0;
109 default:
110 return -ENOTSUP;
111 }
112
113 return -1;
114 }
115
rm3100_convert_raw_to_q31(uint16_t cycle_count,uint32_t raw_reading,q31_t * out,int8_t * shift)116 static int rm3100_convert_raw_to_q31(uint16_t cycle_count, uint32_t raw_reading,
117 q31_t *out, int8_t *shift)
118 {
119 int64_t value;
120 uint8_t divider;
121
122 raw_reading = sys_be24_to_cpu(raw_reading);
123 value = sign_extend(raw_reading, 23);
124
125 /** Convert to Gauss, assuming 1 LSB = 75 uT, given default Cycle-Counting (200).
126 * We can represent the largest sample (2^23 LSB) in Gauss with 11 bits.
127 */
128 if (cycle_count == RM3100_CYCLE_COUNT_DEFAULT) {
129 *shift = 11;
130 divider = 75;
131 } else {
132 /** Otherwise, it's 1 LSB = 38 uT at Cycle-counting for 600 Hz ODR (100):
133 * 12-bits max value.
134 */
135 *shift = 12;
136 divider = 38;
137 }
138
139 int64_t micro_tesla_scaled = ((int64_t)value << (31 - *shift)) / divider;
140 int64_t gauss_scaled = (int64_t)micro_tesla_scaled / 100;
141
142 *out = gauss_scaled;
143
144 return 0;
145 }
146
rm3100_decoder_decode(const uint8_t * buffer,struct sensor_chan_spec chan_spec,uint32_t * fit,uint16_t max_count,void * data_out)147 static int rm3100_decoder_decode(const uint8_t *buffer,
148 struct sensor_chan_spec chan_spec,
149 uint32_t *fit,
150 uint16_t max_count,
151 void *data_out)
152 {
153 struct rm3100_encoded_data *edata = (struct rm3100_encoded_data *)buffer;
154 uint8_t channel_request;
155
156 if (*fit != 0) {
157 return 0;
158 }
159
160 if (max_count == 0 || chan_spec.chan_idx != 0) {
161 return -EINVAL;
162 }
163
164 switch (chan_spec.chan_type) {
165 case SENSOR_CHAN_MAGN_X:
166 case SENSOR_CHAN_MAGN_Y:
167 case SENSOR_CHAN_MAGN_Z: {
168 channel_request = rm3100_encode_channel(chan_spec.chan_type);
169 if ((edata->header.channels & channel_request) != channel_request) {
170 return -ENODATA;
171 }
172
173 struct sensor_q31_data *out = (struct sensor_q31_data *)data_out;
174
175 out->header.base_timestamp_ns = edata->header.timestamp;
176 out->header.reading_count = 1;
177
178 uint32_t raw_reading;
179
180 if (chan_spec.chan_type == SENSOR_CHAN_MAGN_X) {
181 raw_reading = edata->magn.x;
182 } else if (chan_spec.chan_type == SENSOR_CHAN_MAGN_Y) {
183 raw_reading = edata->magn.y;
184 } else {
185 raw_reading = edata->magn.z;
186 }
187
188 rm3100_convert_raw_to_q31(
189 edata->header.cycle_count, raw_reading, &out->readings->value, &out->shift);
190
191 *fit = 1;
192 return 1;
193 }
194 case SENSOR_CHAN_MAGN_XYZ: {
195 channel_request = rm3100_encode_channel(chan_spec.chan_type);
196 if ((edata->header.channels & channel_request) != channel_request) {
197 return -ENODATA;
198 }
199
200 struct sensor_three_axis_data *out = (struct sensor_three_axis_data *)data_out;
201
202 out->header.base_timestamp_ns = edata->header.timestamp;
203 out->header.reading_count = 1;
204
205 rm3100_convert_raw_to_q31(
206 edata->header.cycle_count, edata->magn.x, &out->readings[0].x, &out->shift);
207 rm3100_convert_raw_to_q31(
208 edata->header.cycle_count, edata->magn.y, &out->readings[0].y, &out->shift);
209 rm3100_convert_raw_to_q31(
210 edata->header.cycle_count, edata->magn.z, &out->readings[0].z, &out->shift);
211
212 *fit = 1;
213 return 1;
214 }
215 default:
216 return -EINVAL;
217 }
218
219 return -1;
220 }
221
rm3100_decoder_has_trigger(const uint8_t * buffer,enum sensor_trigger_type trigger)222 static bool rm3100_decoder_has_trigger(const uint8_t *buffer,
223 enum sensor_trigger_type trigger)
224 {
225 return false;
226 }
227
228 SENSOR_DECODER_API_DT_DEFINE() = {
229 .get_frame_count = rm3100_decoder_get_frame_count,
230 .get_size_info = rm3100_decoder_get_size_info,
231 .decode = rm3100_decoder_decode,
232 .has_trigger = rm3100_decoder_has_trigger,
233 };
234
rm3100_get_decoder(const struct device * dev,const struct sensor_decoder_api ** decoder)235 int rm3100_get_decoder(const struct device *dev,
236 const struct sensor_decoder_api **decoder)
237 {
238 ARG_UNUSED(dev);
239
240 *decoder = &SENSOR_DECODER_NAME();
241
242 return 0;
243 }
244