aboutsummaryrefslogtreecommitdiff
path: root/lib/chibios/os/ex/devices/ST/lsm6ds0.c
diff options
context:
space:
mode:
Diffstat (limited to 'lib/chibios/os/ex/devices/ST/lsm6ds0.c')
-rw-r--r--lib/chibios/os/ex/devices/ST/lsm6ds0.c1109
1 files changed, 1109 insertions, 0 deletions
diff --git a/lib/chibios/os/ex/devices/ST/lsm6ds0.c b/lib/chibios/os/ex/devices/ST/lsm6ds0.c
new file mode 100644
index 000000000..0fc05de68
--- /dev/null
+++ b/lib/chibios/os/ex/devices/ST/lsm6ds0.c
@@ -0,0 +1,1109 @@
1/*
2 ChibiOS - Copyright (C) 2016..2019 Rocco Marco Guglielmi
3
4 This file is part of ChibiOS.
5
6 ChibiOS is free software; you can redistribute it and/or modify
7 it under the terms of the GNU General Public License as published by
8 the Free Software Foundation; either version 3 of the License, or
9 (at your option) any later version.
10
11 ChibiOS is distributed in the hope that it will be useful,
12 but WITHOUT ANY WARRANTY; without even the implied warranty of
13 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 GNU General Public License for more details.
15
16 You should have received a copy of the GNU General Public License
17 along with this program. If not, see <http://www.gnu.org/licenses/>.
18
19*/
20
21/**
22 * @file lsm6ds0.c
23 * @brief LSM6DS0 MEMS interface module code.
24 *
25 * @addtogroup LSM6DS0
26 * @ingroup EX_ST
27 * @{
28 */
29
30#include "hal.h"
31#include "lsm6ds0.h"
32
33/*===========================================================================*/
34/* Driver local definitions. */
35/*===========================================================================*/
36
37/*===========================================================================*/
38/* Driver exported variables. */
39/*===========================================================================*/
40
41/*===========================================================================*/
42/* Driver local variables and types. */
43/*===========================================================================*/
44
45/*===========================================================================*/
46/* Driver local functions. */
47/*===========================================================================*/
48
49#if (LSM6DS0_USE_I2C) || defined(__DOXYGEN__)
50/**
51 * @brief Reads registers value using I2C.
52 * @pre The I2C interface must be initialized and the driver started.
53 * @note IF_ADD_INC bit must be 1 in CTRL_REG8
54 *
55 * @param[in] i2cp pointer to the I2C interface
56 * @param[in] sad slave address without R bit
57 * @param[in] reg first sub-register address
58 * @param[out] rxbuf pointer to an output buffer
59 * @param[in] n number of consecutive register to read
60 * @return the operation status.
61 * @notapi
62 */
63msg_t lsm6ds0I2CReadRegister(I2CDriver *i2cp, lsm6ds0_sad_t sad, uint8_t reg,
64 uint8_t* rxbuf, size_t n) {
65
66 return i2cMasterTransmitTimeout(i2cp, sad, &reg, 1, rxbuf, n,
67 TIME_INFINITE);
68}
69
70/**
71 * @brief Writes a value into a register using I2C.
72 * @pre The I2C interface must be initialized and the driver started.
73 *
74 * @param[in] i2cp pointer to the I2C interface
75 * @param[in] sad slave address without R bit
76 * @param[in] txbuf buffer containing sub-address value in first position
77 * and values to write
78 * @param[in] n size of txbuf less one (not considering the first
79 * element)
80 * @return the operation status.
81 * @notapi
82 */
83#define lsm6ds0I2CWriteRegister(i2cp, sad, txbuf, n) \
84 i2cMasterTransmitTimeout(i2cp, sad, txbuf, n + 1, NULL, 0, \
85 TIME_INFINITE)
86#endif /* LSM6DS0_USE_I2C */
87
88/**
89 * @brief Return the number of axes of the BaseAccelerometer.
90 *
91 * @param[in] ip pointer to @p BaseAccelerometer interface.
92 *
93 * @return the number of axes.
94 */
95static size_t acc_get_axes_number(void *ip) {
96 (void)ip;
97
98 return LSM6DS0_ACC_NUMBER_OF_AXES;
99}
100
101/**
102 * @brief Retrieves raw data from the BaseAccelerometer.
103 * @note This data is retrieved from MEMS register without any algebraical
104 * manipulation.
105 * @note The axes array must be at least the same size of the
106 * BaseAccelerometer axes number.
107 *
108 * @param[in] ip pointer to @p BaseAccelerometer interface.
109 * @param[out] axes a buffer which would be filled with raw data.
110 *
111 * @return The operation status.
112 * @retval MSG_OK if the function succeeded.
113 * @retval MSG_RESET if one or more I2C errors occurred, the errors can
114 * be retrieved using @p i2cGetErrors().
115 * @retval MSG_TIMEOUT if a timeout occurred before operation end.
116 */
117static msg_t acc_read_raw(void *ip, int32_t axes[]) {
118 LSM6DS0Driver* devp;
119 uint8_t buff [LSM6DS0_ACC_NUMBER_OF_AXES * 2], i;
120 int16_t tmp;
121 msg_t msg;
122
123 osalDbgCheck((ip != NULL) && (axes != NULL));
124
125 /* Getting parent instance pointer.*/
126 devp = objGetInstance(LSM6DS0Driver*, (BaseAccelerometer*)ip);
127
128 osalDbgAssert((devp->state == LSM6DS0_READY),
129 "acc_read_raw(), invalid state");
130#if LSM6DS0_USE_I2C
131 osalDbgAssert((devp->config->i2cp->state == I2C_READY),
132 "acc_read_raw(), channel not ready");
133
134#if LSM6DS0_SHARED_I2C
135 i2cAcquireBus(devp->config->i2cp);
136 i2cStart(devp->config->i2cp,
137 devp->config->i2ccfg);
138#endif /* LSM6DS0_SHARED_I2C */
139
140 msg = lsm6ds0I2CReadRegister(devp->config->i2cp, devp->config->slaveaddress,
141 LSM6DS0_AD_OUT_X_L_XL, buff,
142 LSM6DS0_ACC_NUMBER_OF_AXES * 2);
143
144#if LSM6DS0_SHARED_I2C
145 i2cReleaseBus(devp->config->i2cp);
146#endif /* LSM6DS0_SHARED_I2C */
147#endif /* LSM6DS0_USE_I2C */
148 if(msg == MSG_OK)
149 for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++) {
150 tmp = buff[2 * i] + (buff[2 * i + 1] << 8);
151 axes[i] = (int32_t)tmp;
152 }
153 return msg;
154}
155
156/**
157 * @brief Retrieves cooked data from the BaseAccelerometer.
158 * @note This data is manipulated according to the formula
159 * cooked = (raw * sensitivity) - bias.
160 * @note Final data is expressed as milli-G.
161 * @note The axes array must be at least the same size of the
162 * BaseAccelerometer axes number.
163 *
164 * @param[in] ip pointer to @p BaseAccelerometer interface.
165 * @param[out] axes a buffer which would be filled with cooked data.
166 *
167 * @return The operation status.
168 * @retval MSG_OK if the function succeeded.
169 * @retval MSG_RESET if one or more I2C errors occurred, the errors can
170 * be retrieved using @p i2cGetErrors().
171 * @retval MSG_TIMEOUT if a timeout occurred before operation end.
172 */
173static msg_t acc_read_cooked(void *ip, float axes[]) {
174 LSM6DS0Driver* devp;
175 uint32_t i;
176 int32_t raw[LSM6DS0_ACC_NUMBER_OF_AXES];
177 msg_t msg;
178
179 osalDbgCheck((ip != NULL) && (axes != NULL));
180
181 /* Getting parent instance pointer.*/
182 devp = objGetInstance(LSM6DS0Driver*, (BaseAccelerometer*)ip);
183
184 osalDbgAssert((devp->state == LSM6DS0_READY),
185 "acc_read_cooked(), invalid state");
186
187 msg = acc_read_raw(ip, raw);
188 for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++) {
189 axes[i] = (raw[i] * devp->accsensitivity[i]) - devp->accbias[i];
190 }
191 return msg;
192}
193
194/**
195 * @brief Set bias values for the BaseAccelerometer.
196 * @note Bias must be expressed as milli-G.
197 * @note The bias buffer must be at least the same size of the
198 * BaseAccelerometer axes number.
199 *
200 * @param[in] ip pointer to @p BaseAccelerometer interface.
201 * @param[in] bp a buffer which contains biases.
202 *
203 * @return The operation status.
204 * @retval MSG_OK if the function succeeded.
205 */
206static msg_t acc_set_bias(void *ip, float *bp) {
207 LSM6DS0Driver* devp;
208 uint32_t i;
209 msg_t msg = MSG_OK;
210
211 osalDbgCheck((ip != NULL) && (bp != NULL));
212
213 /* Getting parent instance pointer.*/
214 devp = objGetInstance(LSM6DS0Driver*, (BaseAccelerometer*)ip);
215
216 osalDbgAssert((devp->state == LSM6DS0_READY),
217 "acc_set_bias(), invalid state");
218
219 for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++) {
220 devp->accbias[i] = bp[i];
221 }
222 return msg;
223}
224
225/**
226 * @brief Reset bias values for the BaseAccelerometer.
227 * @note Default biases value are obtained from device datasheet when
228 * available otherwise they are considered zero.
229 *
230 * @param[in] ip pointer to @p BaseAccelerometer interface.
231 *
232 * @return The operation status.
233 * @retval MSG_OK if the function succeeded.
234 */
235static msg_t acc_reset_bias(void *ip) {
236 LSM6DS0Driver* devp;
237 uint32_t i;
238 msg_t msg = MSG_OK;
239
240 osalDbgCheck(ip != NULL);
241
242 /* Getting parent instance pointer.*/
243 devp = objGetInstance(LSM6DS0Driver*, (BaseAccelerometer*)ip);
244
245 osalDbgAssert((devp->state == LSM6DS0_READY),
246 "acc_reset_bias(), invalid state");
247
248 for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++)
249 devp->accbias[i] = LSM6DS0_ACC_BIAS;
250 return msg;
251}
252
253/**
254 * @brief Set sensitivity values for the BaseAccelerometer.
255 * @note Sensitivity must be expressed as milli-G/LSB.
256 * @note The sensitivity buffer must be at least the same size of the
257 * BaseAccelerometer axes number.
258 *
259 * @param[in] ip pointer to @p BaseAccelerometer interface.
260 * @param[in] sp a buffer which contains sensitivities.
261 *
262 * @return The operation status.
263 * @retval MSG_OK if the function succeeded.
264 */
265static msg_t acc_set_sensivity(void *ip, float *sp) {
266 LSM6DS0Driver* devp;
267 uint32_t i;
268 msg_t msg = MSG_OK;
269
270 /* Getting parent instance pointer.*/
271 devp = objGetInstance(LSM6DS0Driver*, (BaseAccelerometer*)ip);
272
273 osalDbgCheck((ip != NULL) && (sp != NULL));
274
275 osalDbgAssert((devp->state == LSM6DS0_READY),
276 "acc_set_sensivity(), invalid state");
277
278 for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++) {
279 devp->accsensitivity[i] = sp[i];
280 }
281 return msg;
282}
283
284/**
285 * @brief Reset sensitivity values for the BaseAccelerometer.
286 * @note Default sensitivities value are obtained from device datasheet.
287 *
288 * @param[in] ip pointer to @p BaseAccelerometer interface.
289 *
290 * @return The operation status.
291 * @retval MSG_OK if the function succeeded.
292 * @retval MSG_RESET otherwise.
293 */
294static msg_t acc_reset_sensivity(void *ip) {
295 LSM6DS0Driver* devp;
296 uint32_t i;
297 msg_t msg = MSG_OK;
298
299 osalDbgCheck(ip != NULL);
300
301 /* Getting parent instance pointer.*/
302 devp = objGetInstance(LSM6DS0Driver*, (BaseAccelerometer*)ip);
303
304 osalDbgAssert((devp->state == LSM6DS0_READY),
305 "acc_reset_sensivity(), invalid state");
306
307 if(devp->config->accfullscale == LSM6DS0_ACC_FS_2G)
308 for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++)
309 devp->accsensitivity[i] = LSM6DS0_ACC_SENS_2G;
310 else if(devp->config->accfullscale == LSM6DS0_ACC_FS_4G)
311 for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++)
312 devp->accsensitivity[i] = LSM6DS0_ACC_SENS_4G;
313 else if(devp->config->accfullscale == LSM6DS0_ACC_FS_8G)
314 for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++)
315 devp->accsensitivity[i] = LSM6DS0_ACC_SENS_8G;
316 else if(devp->config->accfullscale == LSM6DS0_ACC_FS_16G)
317 for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++)
318 devp->accsensitivity[i] = LSM6DS0_ACC_SENS_16G;
319 else {
320 osalDbgAssert(FALSE, "reset_sensivity(), accelerometer full scale issue");
321 msg = MSG_RESET;
322 }
323 return msg;
324}
325
326/**
327 * @brief Changes the LSM6DS0Driver accelerometer fullscale value.
328 * @note This function also rescale sensitivities and biases based on
329 * previous and next fullscale value.
330 * @note A recalibration is highly suggested after calling this function.
331 *
332 * @param[in] devp pointer to @p LSM6DS0Driver interface.
333 * @param[in] fs new fullscale value.
334 *
335 * @return The operation status.
336 * @retval MSG_OK if the function succeeded.
337 * @retval MSG_RESET otherwise.
338 */
339static msg_t acc_set_full_scale(LSM6DS0Driver *devp, lsm6ds0_acc_fs_t fs) {
340 float newfs, scale;
341 uint8_t i, buff[2];
342 msg_t msg;
343
344 osalDbgCheck(devp != NULL);
345
346 osalDbgAssert((devp->state == LSM6DS0_READY),
347 "acc_set_full_scale(), invalid state");
348 osalDbgAssert((devp->config->i2cp->state == I2C_READY),
349 "acc_set_full_scale(), channel not ready");
350
351 /* Computing new fullscale value.*/
352 if(fs == LSM6DS0_ACC_FS_2G) {
353 newfs = LSM6DS0_ACC_2G;
354 }
355 else if(fs == LSM6DS0_ACC_FS_4G) {
356 newfs = LSM6DS0_ACC_4G;
357 }
358 else if(fs == LSM6DS0_ACC_FS_8G) {
359 newfs = LSM6DS0_ACC_8G;
360 }
361 else if(fs == LSM6DS0_ACC_FS_16G) {
362 newfs = LSM6DS0_ACC_16G;
363 }
364 else {
365 msg = MSG_RESET;
366 return msg;
367 }
368
369 if(newfs != devp->accfullscale) {
370 /* Computing scale value.*/
371 scale = newfs / devp->accfullscale;
372 devp->accfullscale = newfs;
373
374#if LSM6DS0_SHARED_I2C
375 i2cAcquireBus(devp->config->i2cp);
376 i2cStart(devp->config->i2cp,
377 devp->config->i2ccfg);
378#endif /* LSM6DS0_SHARED_I2C */
379
380 /* Updating register.*/
381 msg = lsm6ds0I2CReadRegister(devp->config->i2cp,
382 devp->config->slaveaddress,
383 LSM6DS0_AD_CTRL_REG6_XL, &buff[1], 1);
384
385#if LSM6DS0_SHARED_I2C
386 i2cReleaseBus(devp->config->i2cp);
387#endif /* LSM6DS0_SHARED_I2C */
388
389 if(msg != MSG_OK)
390 return msg;
391
392 buff[1] &= ~(LSM6DS0_CTRL_REG6_XL_FS_MASK);
393 buff[1] |= fs;
394 buff[0] = LSM6DS0_AD_CTRL_REG6_XL;
395
396#if LSM6DS0_SHARED_I2C
397 i2cAcquireBus(devp->config->i2cp);
398 i2cStart(devp->config->i2cp, devp->config->i2ccfg);
399#endif /* LSM6DS0_SHARED_I2C */
400
401 msg = lsm6ds0I2CWriteRegister(devp->config->i2cp,
402 devp->config->slaveaddress, buff, 1);
403
404#if LSM6DS0_SHARED_I2C
405 i2cReleaseBus(devp->config->i2cp);
406#endif /* LSM6DS0_SHARED_I2C */
407
408 if(msg != MSG_OK)
409 return msg;
410
411 /* Scaling sensitivity and bias. Re-calibration is suggested anyway.*/
412 for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++) {
413 devp->accsensitivity[i] *= scale;
414 devp->accbias[i] *= scale;
415 }
416 }
417 return msg;
418}
419
420/**
421 * @brief Return the number of axes of the BaseGyroscope.
422 *
423 * @param[in] ip pointer to @p BaseGyroscope interface.
424 *
425 * @return the number of axes.
426 */
427static size_t gyro_get_axes_number(void *ip) {
428 (void)ip;
429
430 return LSM6DS0_GYRO_NUMBER_OF_AXES;
431}
432
433/**
434 * @brief Retrieves raw data from the BaseGyroscope.
435 * @note This data is retrieved from MEMS register without any algebraical
436 * manipulation.
437 * @note The axes array must be at least the same size of the
438 * BaseGyroscope axes number.
439 *
440 * @param[in] ip pointer to @p BaseGyroscope interface.
441 * @param[out] axes a buffer which would be filled with raw data.
442 *
443 * @return The operation status.
444 * @retval MSG_OK if the function succeeded.
445 * @retval MSG_RESET if one or more I2C errors occurred, the errors can
446 * be retrieved using @p i2cGetErrors().
447 * @retval MSG_TIMEOUT if a timeout occurred before operation end.
448 */
449static msg_t gyro_read_raw(void *ip, int32_t axes[LSM6DS0_GYRO_NUMBER_OF_AXES]) {
450 LSM6DS0Driver* devp;
451 int16_t tmp;
452 uint8_t i, buff [2 * LSM6DS0_GYRO_NUMBER_OF_AXES];
453 msg_t msg = MSG_OK;
454
455 osalDbgCheck((ip != NULL) && (axes != NULL));
456
457 /* Getting parent instance pointer.*/
458 devp = objGetInstance(LSM6DS0Driver*, (BaseGyroscope*)ip);
459
460 osalDbgAssert((devp->state == LSM6DS0_READY),
461 "gyro_read_raw(), invalid state");
462#if LSM6DS0_USE_I2C
463 osalDbgAssert((devp->config->i2cp->state == I2C_READY),
464 "gyro_read_raw(), channel not ready");
465
466#if LSM6DS0_SHARED_I2C
467 i2cAcquireBus(devp->config->i2cp);
468 i2cStart(devp->config->i2cp,
469 devp->config->i2ccfg);
470#endif /* LSM6DS0_SHARED_I2C */
471
472 msg = lsm6ds0I2CReadRegister(devp->config->i2cp, devp->config->slaveaddress,
473 LSM6DS0_AD_OUT_X_L_G, buff,
474 LSM6DS0_GYRO_NUMBER_OF_AXES * 2);
475
476#if LSM6DS0_SHARED_I2C
477 i2cReleaseBus(devp->config->i2cp);
478#endif /* LSM6DS0_SHARED_I2C */
479#endif /* LSM6DS0_USE_I2C */
480
481 for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++) {
482 tmp = buff[2 * i] + (buff[2 * i + 1] << 8);
483 axes[i] = (int32_t)tmp;
484 }
485 return msg;
486}
487
488/**
489 * @brief Retrieves cooked data from the BaseGyroscope.
490 * @note This data is manipulated according to the formula
491 * cooked = (raw * sensitivity) - bias.
492 * @note Final data is expressed as DPS.
493 * @note The axes array must be at least the same size of the
494 * BaseGyroscope axes number.
495 *
496 * @param[in] ip pointer to @p BaseGyroscope interface.
497 * @param[out] axes a buffer which would be filled with cooked data.
498 *
499 * @return The operation status.
500 * @retval MSG_OK if the function succeeded.
501 * @retval MSG_RESET if one or more I2C errors occurred, the errors can
502 * be retrieved using @p i2cGetErrors().
503 * @retval MSG_TIMEOUT if a timeout occurred before operation end.
504 */
505static msg_t gyro_read_cooked(void *ip, float axes[]) {
506 LSM6DS0Driver* devp;
507 uint32_t i;
508 int32_t raw[LSM6DS0_GYRO_NUMBER_OF_AXES];
509 msg_t msg;
510
511 osalDbgCheck((ip != NULL) && (axes != NULL));
512
513 /* Getting parent instance pointer.*/
514 devp = objGetInstance(LSM6DS0Driver*, (BaseGyroscope*)ip);
515
516 osalDbgAssert((devp->state == LSM6DS0_READY),
517 "gyro_read_cooked(), invalid state");
518
519 msg = gyro_read_raw(ip, raw);
520 for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++){
521 axes[i] = (raw[i] * devp->gyrosensitivity[i]) - devp->gyrobias[i];
522 }
523 return msg;
524}
525
526/**
527 * @brief Samples bias values for the BaseGyroscope.
528 * @note The LSM6DS0 shall not be moved during the whole procedure.
529 * @note After this function internal bias is automatically updated.
530 * @note The behavior of this function depends on @p LSM6DS0_BIAS_ACQ_TIMES
531 * and @p LSM6DS0_BIAS_SETTLING_US.
532 *
533 * @param[in] ip pointer to @p BaseGyroscope interface.
534 *
535 * @return The operation status.
536 * @retval MSG_OK if the function succeeded.
537 */
538static msg_t gyro_sample_bias(void *ip) {
539 LSM6DS0Driver* devp;
540 uint32_t i, j;
541 int32_t raw[LSM6DS0_GYRO_NUMBER_OF_AXES];
542 int32_t buff[LSM6DS0_GYRO_NUMBER_OF_AXES] = {0, 0, 0};
543 msg_t msg;
544
545 osalDbgCheck(ip != NULL);
546
547 /* Getting parent instance pointer.*/
548 devp = objGetInstance(LSM6DS0Driver*, (BaseGyroscope*)ip);
549
550 osalDbgAssert((devp->state == LSM6DS0_READY),
551 "gyro_sample_bias(), invalid state");
552#if LSM6DS0_USE_I2C
553 osalDbgAssert((devp->config->i2cp->state == I2C_READY),
554 "gyro_sample_bias(), channel not ready");
555#endif
556
557 for(i = 0; i < LSM6DS0_GYRO_BIAS_ACQ_TIMES; i++){
558 msg = gyro_read_raw(ip, raw);
559 if(msg != MSG_OK)
560 return msg;
561 for(j = 0; j < LSM6DS0_GYRO_NUMBER_OF_AXES; j++){
562 buff[j] += raw[j];
563 }
564 osalThreadSleepMicroseconds(LSM6DS0_GYRO_BIAS_SETTLING_US);
565 }
566
567 for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++){
568 devp->gyrobias[i] = (buff[i] / LSM6DS0_GYRO_BIAS_ACQ_TIMES);
569 devp->gyrobias[i] *= devp->gyrosensitivity[i];
570 }
571 return msg;
572}
573
574/**
575 * @brief Set bias values for the BaseGyroscope.
576 * @note Bias must be expressed as DPS.
577 * @note The bias buffer must be at least the same size of the BaseGyroscope
578 * axes number.
579 *
580 * @param[in] ip pointer to @p BaseGyroscope interface.
581 * @param[in] bp a buffer which contains biases.
582 *
583 * @return The operation status.
584 * @retval MSG_OK if the function succeeded.
585 */
586static msg_t gyro_set_bias(void *ip, float *bp) {
587 LSM6DS0Driver* devp;
588 uint32_t i;
589 msg_t msg = MSG_OK;
590
591 osalDbgCheck((ip != NULL) && (bp != NULL));
592
593 /* Getting parent instance pointer.*/
594 devp = objGetInstance(LSM6DS0Driver*, (BaseGyroscope*)ip);
595
596 osalDbgAssert((devp->state == LSM6DS0_READY),
597 "gyro_set_bias(), invalid state");
598
599 for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++) {
600 devp->gyrobias[i] = bp[i];
601 }
602 return msg;
603}
604
605/**
606 * @brief Reset bias values for the BaseGyroscope.
607 * @note Default biases value are obtained from device datasheet when
608 * available otherwise they are considered zero.
609 *
610 * @param[in] ip pointer to @p BaseGyroscope interface.
611 *
612 * @return The operation status.
613 * @retval MSG_OK if the function succeeded.
614 */
615static msg_t gyro_reset_bias(void *ip) {
616 LSM6DS0Driver* devp;
617 uint32_t i;
618 msg_t msg = MSG_OK;
619
620 osalDbgCheck(ip != NULL);
621
622 /* Getting parent instance pointer.*/
623 devp = objGetInstance(LSM6DS0Driver*, (BaseGyroscope*)ip);
624
625 osalDbgAssert((devp->state == LSM6DS0_READY),
626 "gyro_reset_bias(), invalid state");
627
628 for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++)
629 devp->gyrobias[i] = LSM6DS0_GYRO_BIAS;
630 return msg;
631}
632
633/**
634 * @brief Set sensitivity values for the BaseGyroscope.
635 * @note Sensitivity must be expressed as DPS/LSB.
636 * @note The sensitivity buffer must be at least the same size of the
637 * BaseGyroscope axes number.
638 *
639 * @param[in] ip pointer to @p BaseGyroscope interface.
640 * @param[in] sp a buffer which contains sensitivities.
641 *
642 * @return The operation status.
643 * @retval MSG_OK if the function succeeded.
644 */
645static msg_t gyro_set_sensivity(void *ip, float *sp) {
646 LSM6DS0Driver* devp;
647 uint32_t i;
648 msg_t msg = MSG_OK;
649
650 osalDbgCheck((ip != NULL) && (sp !=NULL));
651
652 /* Getting parent instance pointer.*/
653 devp = objGetInstance(LSM6DS0Driver*, (BaseGyroscope*)ip);
654
655 osalDbgAssert((devp->state == LSM6DS0_READY),
656 "gyro_set_sensivity(), invalid state");
657
658 for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++) {
659 devp->gyrosensitivity[i] = sp[i];
660 }
661 return msg;
662}
663
664/**
665 * @brief Reset sensitivity values for the BaseGyroscope.
666 * @note Default sensitivities value are obtained from device datasheet.
667 *
668 * @param[in] ip pointer to @p BaseGyroscope interface.
669 *
670 * @return The operation status.
671 * @retval MSG_OK if the function succeeded.
672 * @retval MSG_RESET otherwise.
673 */
674static msg_t gyro_reset_sensivity(void *ip) {
675 LSM6DS0Driver* devp;
676 uint32_t i;
677 msg_t msg = MSG_OK;
678
679 osalDbgCheck(ip != NULL);
680
681 /* Getting parent instance pointer.*/
682 devp = objGetInstance(LSM6DS0Driver*, (BaseGyroscope*)ip);
683
684 osalDbgAssert((devp->state == LSM6DS0_READY),
685 "gyro_reset_sensivity(), invalid state");
686
687 if(devp->config->gyrofullscale == LSM6DS0_GYRO_FS_245DPS)
688 for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++)
689 devp->gyrosensitivity[i] = LSM6DS0_GYRO_SENS_245DPS;
690 else if(devp->config->gyrofullscale == LSM6DS0_GYRO_FS_500DPS)
691 for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++)
692 devp->gyrosensitivity[i] = LSM6DS0_GYRO_SENS_500DPS;
693 else if(devp->config->gyrofullscale == LSM6DS0_GYRO_FS_2000DPS)
694 for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++)
695 devp->gyrosensitivity[i] = LSM6DS0_GYRO_SENS_2000DPS;
696 else {
697 osalDbgAssert(FALSE, "gyro_reset_sensivity(), full scale issue");
698 return MSG_RESET;
699 }
700 return msg;
701}
702
703/**
704 * @brief Changes the LSM6DS0Driver gyroscope fullscale value.
705 * @note This function also rescale sensitivities and biases based on
706 * previous and next fullscale value.
707 * @note A recalibration is highly suggested after calling this function.
708 *
709 * @param[in] devp pointer to @p BaseGyroscope interface.
710 * @param[in] fs new fullscale value.
711 *
712 * @return The operation status.
713 * @retval MSG_OK if the function succeeded.
714 * @retval MSG_RESET otherwise.
715 */
716static msg_t gyro_set_full_scale(LSM6DS0Driver *devp, lsm6ds0_gyro_fs_t fs) {
717 float newfs, scale;
718 uint8_t i, buff[2];
719 msg_t msg = MSG_OK;
720
721 osalDbgCheck(devp != NULL);
722
723 osalDbgAssert((devp->state == LSM6DS0_READY),
724 "gyro_set_full_scale(), invalid state");
725#if LSM6DS0_USE_I2C
726 osalDbgAssert((devp->config->i2cp->state == I2C_READY),
727 "gyro_set_full_scale(), channel not ready");
728#endif
729
730 if(fs == LSM6DS0_GYRO_FS_245DPS) {
731 newfs = LSM6DS0_GYRO_245DPS;
732 }
733 else if(fs == LSM6DS0_GYRO_FS_500DPS) {
734 newfs = LSM6DS0_GYRO_500DPS;
735 }
736 else if(fs == LSM6DS0_GYRO_FS_2000DPS) {
737 newfs = LSM6DS0_GYRO_2000DPS;
738 }
739 else {
740 return MSG_RESET;
741 }
742
743 if(newfs != devp->gyrofullscale) {
744 scale = newfs / devp->gyrofullscale;
745 devp->gyrofullscale = newfs;
746
747#if LSM6DS0_USE_I2C
748#if LSM6DS0_SHARED_I2C
749 i2cAcquireBus(devp->config->i2cp);
750 i2cStart(devp->config->i2cp,
751 devp->config->i2ccfg);
752#endif /* LSM6DS0_SHARED_I2C */
753
754 /* Updating register.*/
755 msg = lsm6ds0I2CReadRegister(devp->config->i2cp,
756 devp->config->slaveaddress,
757 LSM6DS0_AD_CTRL_REG1_G, &buff[1], 1);
758
759#if LSM6DS0_SHARED_I2C
760 i2cReleaseBus(devp->config->i2cp);
761#endif /* LSM6DS0_SHARED_I2C */
762#endif /* LSM6DS0_USE_I2C */
763
764 buff[1] &= ~(LSM6DS0_CTRL_REG1_G_FS_MASK);
765 buff[1] |= fs;
766 buff[0] = LSM6DS0_AD_CTRL_REG1_G;
767
768#if LSM6DS0_USE_I2C
769#if LSM6DS0_SHARED_I2C
770 i2cAcquireBus(devp->config->i2cp);
771 i2cStart(devp->config->i2cp,
772 devp->config->i2ccfg);
773#endif /* LSM6DS0_SHARED_I2C */
774
775 lsm6ds0I2CWriteRegister(devp->config->i2cp, devp->config->slaveaddress,
776 buff, 1);
777
778#if LSM6DS0_SHARED_I2C
779 i2cReleaseBus(devp->config->i2cp);
780#endif /* LSM6DS0_SHARED_I2C */
781#endif /* LSM6DS0_USE_I2C */
782
783 /* Scaling sensitivity and bias. Re-calibration is suggested anyway. */
784 for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++) {
785 devp->gyrosensitivity[i] *= scale;
786 devp->gyrobias[i] *= scale;
787 }
788 }
789 return msg;
790}
791
792static const struct LSM6DS0VMT vmt_device = {
793 (size_t)0,
794 acc_set_full_scale, gyro_set_full_scale
795};
796
797static const struct BaseAccelerometerVMT vmt_accelerometer = {
798 sizeof(struct LSM6DS0VMT*),
799 acc_get_axes_number, acc_read_raw, acc_read_cooked,
800 acc_set_bias, acc_reset_bias, acc_set_sensivity, acc_reset_sensivity
801};
802
803static const struct BaseGyroscopeVMT vmt_gyroscope = {
804 sizeof(struct LSM6DS0VMT*) + sizeof(BaseAccelerometer),
805 gyro_get_axes_number, gyro_read_raw, gyro_read_cooked,
806 gyro_sample_bias, gyro_set_bias, gyro_reset_bias,
807 gyro_set_sensivity, gyro_reset_sensivity
808};
809
810/*===========================================================================*/
811/* Driver exported functions. */
812/*===========================================================================*/
813
814/**
815 * @brief Initializes an instance.
816 *
817 * @param[out] devp pointer to the @p LSM6DS0Driver object
818 *
819 * @init
820 */
821void lsm6ds0ObjectInit(LSM6DS0Driver *devp) {
822 devp->vmt = &vmt_device;
823 devp->acc_if.vmt = &vmt_accelerometer;
824 devp->gyro_if.vmt = &vmt_gyroscope;
825
826 devp->config = NULL;
827
828 devp->accaxes = LSM6DS0_ACC_NUMBER_OF_AXES;
829 devp->gyroaxes = LSM6DS0_GYRO_NUMBER_OF_AXES;
830
831 devp->state = LSM6DS0_STOP;
832}
833
834/**
835 * @brief Configures and activates LSM6DS0 Complex Driver peripheral.
836 *
837 * @param[in] devp pointer to the @p LSM6DS0Driver object
838 * @param[in] config pointer to the @p LSM6DS0Config object
839 *
840 * @api
841 */
842void lsm6ds0Start(LSM6DS0Driver *devp, const LSM6DS0Config *config) {
843 uint32_t i;
844 uint8_t cr[5];
845 osalDbgCheck((devp != NULL) && (config != NULL));
846
847 osalDbgAssert((devp->state == LSM6DS0_STOP) ||
848 (devp->state == LSM6DS0_READY),
849 "lsm6ds0Start(), invalid state");
850
851 devp->config = config;
852
853 /* Configuring common registers.*/
854
855 /* Control register 8 configuration block.*/
856 {
857 cr[0] = LSM6DS0_AD_CTRL_REG8;
858 cr[1] = LSM6DS0_CTRL_REG8_IF_ADD_INC;
859#if LSM6DS0_USE_ADVANCED || defined(__DOXYGEN__)
860 cr[1] |= devp->config->endianness | devp->config->blockdataupdate;
861#endif
862 }
863#if LSM6DS0_USE_I2C
864#if LSM6DS0_SHARED_I2C
865 i2cAcquireBus(devp->config->i2cp);
866#endif /* LSM6DS0_SHARED_I2C */
867
868 i2cStart(devp->config->i2cp, devp->config->i2ccfg);
869 lsm6ds0I2CWriteRegister(devp->config->i2cp, devp->config->slaveaddress,
870 cr, 1);
871
872#if LSM6DS0_SHARED_I2C
873 i2cReleaseBus(devp->config->i2cp);
874#endif /* LSM6DS0_SHARED_I2C */
875#endif /* LSM6DS0_USE_I2C */
876
877 /* Configuring Accelerometer subsystem.*/
878 /* Multiple write starting address.*/
879 cr[0] = LSM6DS0_AD_CTRL_REG5_XL;
880 /* Control register 5 configuration block.*/
881 {
882 cr[1] = LSM6DS0_CTRL_REG5_XL_XEN_XL | LSM6DS0_CTRL_REG5_XL_YEN_XL |
883 LSM6DS0_CTRL_REG5_XL_ZEN_XL;
884#if LSM6DS0_USE_ADVANCED || defined(__DOXYGEN__)
885 cr[1] |= devp->config->accdecmode;
886#endif
887 }
888
889 /* Control register 6 configuration block.*/
890 {
891 cr[2] = devp->config->accoutdatarate |
892 devp->config->accfullscale;
893 }
894
895#if LSM6DS0_USE_I2C
896#if LSM6DS0_SHARED_I2C
897 i2cAcquireBus(devp->config->i2cp);
898 i2cStart(devp->config->i2cp, devp->config->i2ccfg);
899#endif /* LSM6DS0_SHARED_I2C */
900
901 lsm6ds0I2CWriteRegister(devp->config->i2cp, devp->config->slaveaddress,
902 cr, 2);
903
904#if LSM6DS0_SHARED_I2C
905 i2cReleaseBus(devp->config->i2cp);
906#endif /* LSM6DS0_SHARED_I2C */
907#endif /* LSM6DS0_USE_I2C */
908
909 /* Storing sensitivity according to user settings */
910 if(devp->config->accfullscale == LSM6DS0_ACC_FS_2G) {
911 for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++) {
912 if(devp->config->accsensitivity == NULL)
913 devp->accsensitivity[i] = LSM6DS0_ACC_SENS_2G;
914 else
915 devp->accsensitivity[i] = devp->config->accsensitivity[i];
916 }
917 devp->accfullscale = LSM6DS0_ACC_2G;
918 }
919 else if(devp->config->accfullscale == LSM6DS0_ACC_FS_4G) {
920 for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++) {
921 if(devp->config->accsensitivity == NULL)
922 devp->accsensitivity[i] = LSM6DS0_ACC_SENS_4G;
923 else
924 devp->accsensitivity[i] = devp->config->accsensitivity[i];
925 }
926 devp->accfullscale = LSM6DS0_ACC_4G;
927 }
928 else if(devp->config->accfullscale == LSM6DS0_ACC_FS_8G) {
929 for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++) {
930 if(devp->config->accsensitivity == NULL)
931 devp->accsensitivity[i] = LSM6DS0_ACC_SENS_8G;
932 else
933 devp->accsensitivity[i] = devp->config->accsensitivity[i];
934 }
935 devp->accfullscale = LSM6DS0_ACC_8G;
936 }
937 else if(devp->config->accfullscale == LSM6DS0_ACC_FS_16G) {
938 for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++) {
939 if(devp->config->accsensitivity == NULL)
940 devp->accsensitivity[i] = LSM6DS0_ACC_SENS_16G;
941 else
942 devp->accsensitivity[i] = devp->config->accsensitivity[i];
943 }
944 devp->accfullscale = LSM6DS0_ACC_16G;
945 }
946 else
947 osalDbgAssert(FALSE, "lsm6ds0Start(), accelerometer full scale issue");
948
949 /* Storing bias information */
950 if(devp->config->accbias != NULL)
951 for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++)
952 devp->accbias[i] = devp->config->accbias[i];
953 else
954 for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++)
955 devp->accbias[i] = LSM6DS0_ACC_BIAS;
956
957 /* Configuring Gyroscope subsystem.*/
958 /* Multiple write starting address.*/
959 cr[0] = LSM6DS0_AD_CTRL_REG1_G;
960
961 /* Control register 1 configuration block.*/
962 {
963 cr[1] = devp->config->gyrofullscale |
964 devp->config->gyrooutdatarate;
965 }
966
967 /* Control register 2 configuration block.*/
968 {
969 cr[2] = 0;
970#if LSM6DS0_USE_ADVANCED || defined(__DOXYGEN__)
971 cr[2] |= devp->config->gyrooutsel;
972#endif
973 }
974
975 /* Control register 3 configuration block.*/
976 {
977 cr[3] = 0;
978#if LSM6DS0_USE_ADVANCED || defined(__DOXYGEN__)
979 cr[3] |= devp->config->gyrohpfenable |
980 devp->config->gyrolowmodecfg |
981 devp->config->gyrohpcfg;
982#endif
983 }
984
985 /* Control register 4 configuration block.*/
986 {
987 cr[4] = LSM6DS0_CTRL_REG4_XEN_G | LSM6DS0_CTRL_REG4_YEN_G |
988 LSM6DS0_CTRL_REG4_ZEN_G;
989 }
990#if LSM6DS0_USE_I2C
991#if LSM6DS0_SHARED_I2C
992 i2cAcquireBus(devp->config->i2cp);
993 i2cStart(devp->config->i2cp, devp->config->i2ccfg);
994#endif /* LSM6DS0_SHARED_I2C */
995
996 lsm6ds0I2CWriteRegister(devp->config->i2cp, devp->config->slaveaddress,
997 cr, 4);
998
999#if LSM6DS0_SHARED_I2C
1000 i2cReleaseBus(devp->config->i2cp);
1001#endif /* LSM6DS0_SHARED_I2C */
1002#endif /* LSM6DS0_USE_I2C */
1003
1004 cr[0] = LSM6DS0_AD_CTRL_REG9;
1005 /* Control register 9 configuration block.*/
1006 {
1007 cr[1] = 0;
1008 }
1009#if LSM6DS0_USE_I2C
1010#if LSM6DS0_SHARED_I2C
1011 i2cAcquireBus(devp->config->i2cp);
1012 i2cStart(devp->config->i2cp, devp->config->i2ccfg);
1013#endif /* LSM6DS0_SHARED_I2C */
1014
1015 lsm6ds0I2CWriteRegister(devp->config->i2cp, devp->config->slaveaddress,
1016 cr, 1);
1017
1018#if LSM6DS0_SHARED_I2C
1019 i2cReleaseBus(devp->config->i2cp);
1020#endif /* LSM6DS0_SHARED_I2C */
1021#endif /* LSM6DS0_USE_I2C */
1022
1023 if(devp->config->gyrofullscale == LSM6DS0_GYRO_FS_245DPS) {
1024 for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++) {
1025 if(devp->config->gyrosensitivity == NULL)
1026 devp->gyrosensitivity[i] = LSM6DS0_GYRO_SENS_245DPS;
1027 else
1028 devp->gyrosensitivity[i] = devp->config->gyrosensitivity[i];
1029 }
1030 devp->gyrofullscale = LSM6DS0_GYRO_245DPS;
1031 }
1032 else if(devp->config->gyrofullscale == LSM6DS0_GYRO_FS_500DPS) {
1033 for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++) {
1034 if(devp->config->gyrosensitivity == NULL)
1035 devp->gyrosensitivity[i] = LSM6DS0_GYRO_SENS_500DPS;
1036 else
1037 devp->gyrosensitivity[i] = devp->config->gyrosensitivity[i];
1038 }
1039 devp->gyrofullscale = LSM6DS0_GYRO_500DPS;
1040 }
1041 else if(devp->config->gyrofullscale == LSM6DS0_GYRO_FS_2000DPS) {
1042 for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++) {
1043 if(devp->config->gyrosensitivity == NULL)
1044 devp->gyrosensitivity[i] = LSM6DS0_GYRO_SENS_2000DPS;
1045 else
1046 devp->gyrosensitivity[i] = devp->config->gyrosensitivity[i];
1047 }
1048 devp->gyrofullscale = LSM6DS0_GYRO_2000DPS;
1049 }
1050 else
1051 osalDbgAssert(FALSE, "lsm6ds0Start(), gyroscope full scale issue");
1052
1053 /* Storing bias information */
1054 if(devp->config->gyrobias != NULL)
1055 for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++)
1056 devp->gyrobias[i] = devp->config->gyrobias[i];
1057 else
1058 for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++)
1059 devp->gyrobias[i] = LSM6DS0_GYRO_BIAS;
1060
1061 /* This is the MEMS transient recovery time */
1062 osalThreadSleepMilliseconds(5);
1063
1064 devp->state = LSM6DS0_READY;
1065}
1066
1067/**
1068 * @brief Deactivates the LSM6DS0 Complex Driver peripheral.
1069 *
1070 * @param[in] devp pointer to the @p LSM6DS0Driver object
1071 *
1072 * @api
1073 */
1074void lsm6ds0Stop(LSM6DS0Driver *devp) {
1075 uint8_t cr[2];
1076
1077 osalDbgCheck(devp != NULL);
1078
1079 osalDbgAssert((devp->state == LSM6DS0_STOP) || (devp->state == LSM6DS0_READY),
1080 "lsm6ds0Stop(), invalid state");
1081
1082 if (devp->state == LSM6DS0_READY) {
1083#if LSM6DS0_USE_I2C
1084#if LSM6DS0_SHARED_I2C
1085 i2cAcquireBus(devp->config->i2cp);
1086 i2cStart(devp->config->i2cp, devp->config->i2ccfg);
1087#endif /* LSM6DS0_SHARED_I2C */
1088
1089 /* Disabling accelerometer.*/
1090 cr[0] = LSM6DS0_AD_CTRL_REG6_XL;
1091 cr[1] = 0;
1092 lsm6ds0I2CWriteRegister(devp->config->i2cp, devp->config->slaveaddress,
1093 cr, 1);
1094
1095 /* Disabling gyroscope.*/
1096 cr[0] = LSM6DS0_AD_CTRL_REG9;
1097 cr[1] = LSM6DS0_CTRL_REG9_SLEEP_G;
1098 lsm6ds0I2CWriteRegister(devp->config->i2cp, devp->config->slaveaddress,
1099 cr, 1);
1100
1101 i2cStop(devp->config->i2cp);
1102#if LSM6DS0_SHARED_I2C
1103 i2cReleaseBus(devp->config->i2cp);
1104#endif /* LSM6DS0_SHARED_I2C */
1105#endif /* LSM6DS0_USE_I2C */
1106 }
1107 devp->state = LSM6DS0_STOP;
1108}
1109/** @} */
generated by cgit v1.2.3 (git 2.25.1) at 2025-08-30 20:21:21 +0100