1 /*
2 * Copyright (c) 2020 TDK Invensense
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <zephyr/sys/byteorder.h>
8 #include <zephyr/drivers/sensor.h>
9 #include <zephyr/logging/log.h>
10
11 #include "icm42605.h"
12 #include "icm42605_reg.h"
13 #include "icm42605_spi.h"
14
15 LOG_MODULE_DECLARE(ICM42605, CONFIG_SENSOR_LOG_LEVEL);
16
icm42605_set_fs(const struct device * dev,uint16_t a_sf,uint16_t g_sf)17 int icm42605_set_fs(const struct device *dev, uint16_t a_sf, uint16_t g_sf)
18 {
19 const struct icm42605_config *cfg = dev->config;
20 uint8_t databuf;
21 int result;
22
23 result = inv_spi_read(&cfg->spi, REG_ACCEL_CONFIG0, &databuf, 1);
24 if (result) {
25 return result;
26 }
27 databuf &= ~BIT_ACCEL_FSR;
28
29 databuf |= a_sf;
30
31 result = inv_spi_single_write(&cfg->spi, REG_ACCEL_CONFIG0, &databuf);
32
33 result = inv_spi_read(&cfg->spi, REG_GYRO_CONFIG0, &databuf, 1);
34
35 if (result) {
36 return result;
37 }
38
39 databuf &= ~BIT_GYRO_FSR;
40 databuf |= g_sf;
41
42 result = inv_spi_single_write(&cfg->spi, REG_GYRO_CONFIG0, &databuf);
43
44 if (result) {
45 return result;
46 }
47
48 return 0;
49 }
50
icm42605_set_odr(const struct device * dev,uint16_t a_rate,uint16_t g_rate)51 int icm42605_set_odr(const struct device *dev, uint16_t a_rate, uint16_t g_rate)
52 {
53 const struct icm42605_config *cfg = dev->config;
54 uint8_t databuf;
55 int result;
56
57 if (a_rate > 8000 || g_rate > 8000 ||
58 a_rate < 1 || g_rate < 12) {
59 LOG_ERR("Not supported frequency");
60 return -ENOTSUP;
61 }
62
63 result = inv_spi_read(&cfg->spi, REG_ACCEL_CONFIG0, &databuf, 1);
64
65 if (result) {
66 return result;
67 }
68
69 databuf &= ~BIT_ACCEL_ODR;
70
71 if (a_rate > 4000) {
72 databuf |= BIT_ACCEL_ODR_8000;
73 } else if (a_rate > 2000) {
74 databuf |= BIT_ACCEL_ODR_4000;
75 } else if (a_rate > 1000) {
76 databuf |= BIT_ACCEL_ODR_2000;
77 } else if (a_rate > 500) {
78 databuf |= BIT_ACCEL_ODR_1000;
79 } else if (a_rate > 200) {
80 databuf |= BIT_ACCEL_ODR_500;
81 } else if (a_rate > 100) {
82 databuf |= BIT_ACCEL_ODR_200;
83 } else if (a_rate > 50) {
84 databuf |= BIT_ACCEL_ODR_100;
85 } else if (a_rate > 25) {
86 databuf |= BIT_ACCEL_ODR_50;
87 } else if (a_rate > 12) {
88 databuf |= BIT_ACCEL_ODR_25;
89 } else if (a_rate > 6) {
90 databuf |= BIT_ACCEL_ODR_12;
91 } else if (a_rate > 3) {
92 databuf |= BIT_ACCEL_ODR_6;
93 } else if (a_rate > 1) {
94 databuf |= BIT_ACCEL_ODR_3;
95 } else {
96 databuf |= BIT_ACCEL_ODR_1;
97 }
98
99 result = inv_spi_single_write(&cfg->spi, REG_ACCEL_CONFIG0, &databuf);
100
101 if (result) {
102 return result;
103 }
104
105 LOG_DBG("Write Accel ODR 0x%X", databuf);
106
107 result = inv_spi_read(&cfg->spi, REG_GYRO_CONFIG0, &databuf, 1);
108
109 if (result) {
110 return result;
111 }
112
113 databuf &= ~BIT_GYRO_ODR;
114
115 if (g_rate > 4000) {
116 databuf |= BIT_GYRO_ODR_8000;
117 } else if (g_rate > 2000) {
118 databuf |= BIT_GYRO_ODR_4000;
119 } else if (g_rate > 1000) {
120 databuf |= BIT_GYRO_ODR_2000;
121 } else if (g_rate > 500) {
122 databuf |= BIT_GYRO_ODR_1000;
123 } else if (g_rate > 200) {
124 databuf |= BIT_GYRO_ODR_500;
125 } else if (g_rate > 100) {
126 databuf |= BIT_GYRO_ODR_200;
127 } else if (g_rate > 50) {
128 databuf |= BIT_GYRO_ODR_100;
129 } else if (g_rate > 25) {
130 databuf |= BIT_GYRO_ODR_50;
131 } else if (g_rate > 12) {
132 databuf |= BIT_GYRO_ODR_25;
133 } else {
134 databuf |= BIT_GYRO_ODR_12;
135 }
136
137 LOG_DBG("Write GYRO ODR 0x%X", databuf);
138
139 result = inv_spi_single_write(&cfg->spi, REG_GYRO_CONFIG0, &databuf);
140 if (result) {
141 return result;
142 }
143
144 return result;
145 }
146
icm42605_sensor_init(const struct device * dev)147 int icm42605_sensor_init(const struct device *dev)
148 {
149 const struct icm42605_config *cfg = dev->config;
150 int result = 0;
151 uint8_t v;
152
153 result = inv_spi_read(&cfg->spi, REG_WHO_AM_I, &v, 1);
154
155 if (result) {
156 return result;
157 }
158
159 LOG_DBG("WHO AM I : 0x%X", v);
160
161 result = inv_spi_read(&cfg->spi, REG_DEVICE_CONFIG, &v, 1);
162
163 if (result) {
164 LOG_DBG("read REG_DEVICE_CONFIG_REG failed");
165 return result;
166 }
167
168 v |= BIT_SOFT_RESET;
169
170 result = inv_spi_single_write(&cfg->spi, REG_DEVICE_CONFIG, &v);
171
172 if (result) {
173 LOG_ERR("write REG_DEVICE_CONFIG failed");
174 return result;
175 }
176
177 /* Need at least 10ms after soft reset */
178 k_msleep(10);
179
180 v = BIT_GYRO_AFSR_MODE_HFS | BIT_ACCEL_AFSR_MODE_HFS | BIT_CLK_SEL_PLL;
181
182 result = inv_spi_single_write(&cfg->spi, REG_INTF_CONFIG1, &v);
183
184 if (result) {
185 LOG_ERR("write REG_INTF_CONFIG1 failed");
186 return result;
187 }
188
189 v = BIT_EN_DREG_FIFO_D2A |
190 BIT_TMST_TO_REGS_EN |
191 BIT_TMST_EN;
192
193 result = inv_spi_single_write(&cfg->spi, REG_TMST_CONFIG, &v);
194
195 if (result) {
196 LOG_ERR("Write REG_TMST_CONFIG failed");
197 return result;
198 }
199
200 result = inv_spi_read(&cfg->spi, REG_INTF_CONFIG0, &v, 1);
201
202 if (result) {
203 LOG_ERR("Read REG_INTF_CONFIG0 failed");
204 return result;
205 }
206
207 LOG_DBG("Read REG_INTF_CONFIG0 0x%X", v);
208
209 v |= BIT_UI_SIFS_DISABLE_I2C;
210
211 result = inv_spi_single_write(&cfg->spi, REG_INTF_CONFIG0, &v);
212
213 if (result) {
214 LOG_ERR("Write REG_INTF_CONFIG failed");
215 return result;
216 }
217
218 v = 0;
219 result = inv_spi_single_write(&cfg->spi, REG_INT_CONFIG1, &v);
220
221 if (result) {
222 return result;
223 }
224
225 result = inv_spi_single_write(&cfg->spi, REG_PWR_MGMT0, &v);
226
227 if (result) {
228 return result;
229 }
230
231 return 0;
232 }
233
icm42605_turn_on_fifo(const struct device * dev)234 int icm42605_turn_on_fifo(const struct device *dev)
235 {
236 const struct icm42605_data *drv_data = dev->data;
237 const struct icm42605_config *cfg = dev->config;
238
239 uint8_t int0_en = BIT_INT_UI_DRDY_INT1_EN;
240 uint8_t fifo_en = BIT_FIFO_ACCEL_EN | BIT_FIFO_GYRO_EN | BIT_FIFO_WM_TH;
241 uint8_t burst_read[3];
242 int result;
243 uint8_t v = 0;
244
245 v = BIT_FIFO_MODE_BYPASS;
246 result = inv_spi_single_write(&cfg->spi, REG_FIFO_CONFIG, &v);
247 if (result) {
248 return result;
249 }
250
251 v = 0;
252 result = inv_spi_single_write(&cfg->spi, REG_FIFO_CONFIG1, &v);
253 if (result) {
254 return result;
255 }
256
257 result = inv_spi_read(&cfg->spi, REG_FIFO_COUNTH, burst_read, 2);
258 if (result) {
259 return result;
260 }
261
262 result = inv_spi_read(&cfg->spi, REG_FIFO_DATA, burst_read, 3);
263 if (result) {
264 return result;
265 }
266
267 v = BIT_FIFO_MODE_STREAM;
268 result = inv_spi_single_write(&cfg->spi, REG_FIFO_CONFIG, &v);
269 if (result) {
270 return result;
271 }
272
273 result = inv_spi_single_write(&cfg->spi, REG_FIFO_CONFIG1, &fifo_en);
274 if (result) {
275 return result;
276 }
277
278 result = inv_spi_single_write(&cfg->spi, REG_INT_SOURCE0, &int0_en);
279 if (result) {
280 return result;
281 }
282
283 if (drv_data->tap_en) {
284 v = BIT_TAP_ENABLE;
285 result = inv_spi_single_write(&cfg->spi, REG_APEX_CONFIG0, &v);
286 if (result) {
287 return result;
288 }
289
290 v = BIT_DMP_INIT_EN;
291 result = inv_spi_single_write(&cfg->spi, REG_SIGNAL_PATH_RESET, &v);
292 if (result) {
293 return result;
294 }
295
296 v = BIT_BANK_SEL_4;
297 result = inv_spi_single_write(&cfg->spi, REG_BANK_SEL, &v);
298 if (result) {
299 return result;
300 }
301
302 v = BIT_INT_STATUS_TAP_DET;
303 result = inv_spi_single_write(&cfg->spi, REG_INT_SOURCE6, &v);
304 if (result) {
305 return result;
306 }
307
308 v = BIT_BANK_SEL_0;
309 result = inv_spi_single_write(&cfg->spi, REG_BANK_SEL, &v);
310 if (result) {
311 return result;
312 }
313 }
314
315 LOG_DBG("turn on fifo done");
316 return 0;
317 }
318
icm42605_turn_off_fifo(const struct device * dev)319 int icm42605_turn_off_fifo(const struct device *dev)
320 {
321 const struct icm42605_data *drv_data = dev->data;
322 const struct icm42605_config *cfg = dev->config;
323 uint8_t int0_en = 0;
324 uint8_t burst_read[3];
325 int result;
326 uint8_t v = 0;
327
328 v = BIT_FIFO_MODE_BYPASS;
329 result = inv_spi_single_write(&cfg->spi, REG_FIFO_CONFIG, &v);
330 if (result) {
331 return result;
332 }
333
334 v = 0;
335 result = inv_spi_single_write(&cfg->spi, REG_FIFO_CONFIG1, &v);
336 if (result) {
337 return result;
338 }
339
340 result = inv_spi_read(&cfg->spi, REG_FIFO_COUNTH, burst_read, 2);
341 if (result) {
342 return result;
343 }
344
345 result = inv_spi_read(&cfg->spi, REG_FIFO_DATA, burst_read, 3);
346 if (result) {
347 return result;
348 }
349
350 result = inv_spi_single_write(&cfg->spi, REG_INT_SOURCE0, &int0_en);
351 if (result) {
352 return result;
353 }
354
355 if (drv_data->tap_en) {
356 v = 0;
357 result = inv_spi_single_write(&cfg->spi, REG_APEX_CONFIG0, &v);
358 if (result) {
359 return result;
360 }
361
362 result = inv_spi_single_write(&cfg->spi, REG_SIGNAL_PATH_RESET, &v);
363 if (result) {
364 return result;
365 }
366
367 v = BIT_BANK_SEL_4;
368 result = inv_spi_single_write(&cfg->spi, REG_BANK_SEL, &v);
369 if (result) {
370 return result;
371 }
372
373 v = 0;
374 result = inv_spi_single_write(&cfg->spi, REG_INT_SOURCE6, &v);
375 if (result) {
376 return result;
377 }
378
379 v = BIT_BANK_SEL_0;
380 result = inv_spi_single_write(&cfg->spi, REG_BANK_SEL, &v);
381 if (result) {
382 return result;
383 }
384 }
385
386 return 0;
387 }
388
icm42605_turn_on_sensor(const struct device * dev)389 int icm42605_turn_on_sensor(const struct device *dev)
390 {
391 struct icm42605_data *drv_data = dev->data;
392 const struct icm42605_config *cfg = dev->config;
393 uint8_t v = 0;
394 int result = 0;
395
396
397 if (drv_data->sensor_started) {
398 LOG_ERR("Sensor already started");
399 return -EALREADY;
400 }
401
402 icm42605_set_fs(dev, drv_data->accel_sf, drv_data->gyro_sf);
403
404 icm42605_set_odr(dev, drv_data->accel_hz, drv_data->gyro_hz);
405
406 v |= BIT_ACCEL_MODE_LNM;
407 v |= BIT_GYRO_MODE_LNM;
408
409 result = inv_spi_single_write(&cfg->spi, REG_PWR_MGMT0, &v);
410 if (result) {
411 return result;
412 }
413
414 /* Accelerometer sensor need at least 10ms startup time
415 * Gyroscope sensor need at least 30ms startup time
416 */
417 k_msleep(100);
418
419 icm42605_turn_on_fifo(dev);
420
421 drv_data->sensor_started = true;
422
423 return 0;
424 }
425
icm42605_turn_off_sensor(const struct device * dev)426 int icm42605_turn_off_sensor(const struct device *dev)
427 {
428 const struct icm42605_config *cfg = dev->config;
429 uint8_t v = 0;
430 int result = 0;
431
432 result = inv_spi_read(&cfg->spi, REG_PWR_MGMT0, &v, 1);
433
434 v ^= BIT_ACCEL_MODE_LNM;
435 v ^= BIT_GYRO_MODE_LNM;
436
437 result = inv_spi_single_write(&cfg->spi, REG_PWR_MGMT0, &v);
438 if (result) {
439 return result;
440 }
441
442 /* Accelerometer sensor need at least 10ms startup time
443 * Gyroscope sensor need at least 30ms startup time
444 */
445 k_msleep(100);
446
447 icm42605_turn_off_fifo(dev);
448
449 return 0;
450 }
451
