aboutsummaryrefslogtreecommitdiff
path: root/lib/chibios/os/ex/devices/ST/lsm6dsl.c
diff options
context:
space:
mode:
Diffstat (limited to 'lib/chibios/os/ex/devices/ST/lsm6dsl.c')
-rw-r--r--lib/chibios/os/ex/devices/ST/lsm6dsl.c1119
1 files changed, 1119 insertions, 0 deletions
diff --git a/lib/chibios/os/ex/devices/ST/lsm6dsl.c b/lib/chibios/os/ex/devices/ST/lsm6dsl.c
new file mode 100644
index 000000000..adb0d7a4d
--- /dev/null
+++ b/lib/chibios/os/ex/devices/ST/lsm6dsl.c
@@ -0,0 +1,1119 @@
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 lsm6dsl.c
23 * @brief LSM6DSL MEMS interface module code.
24 *
25 * @addtogroup LSM6DSL
26 * @ingroup EX_ST
27 * @{
28 */
29
30#include "hal.h"
31#include "lsm6dsl.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 (LSM6DSL_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 lsm6dslI2CReadRegister(I2CDriver *i2cp, lsm6dsl_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 lsm6dslI2CWriteRegister(i2cp, sad, txbuf, n) \
84 i2cMasterTransmitTimeout(i2cp, sad, txbuf, n + 1, NULL, 0, \
85 TIME_INFINITE)
86#endif /* LSM6DSL_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 LSM6DSL_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 LSM6DSLDriver* devp;
119 uint8_t buff [LSM6DSL_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(LSM6DSLDriver*, (BaseAccelerometer*)ip);
127
128 osalDbgAssert((devp->state == LSM6DSL_READY),
129 "acc_read_raw(), invalid state");
130#if LSM6DSL_USE_I2C
131 osalDbgAssert((devp->config->i2cp->state == I2C_READY),
132 "acc_read_raw(), channel not ready");
133
134#if LSM6DSL_SHARED_I2C
135 i2cAcquireBus(devp->config->i2cp);
136 i2cStart(devp->config->i2cp,
137 devp->config->i2ccfg);
138#endif /* LSM6DSL_SHARED_I2C */
139
140 msg = lsm6dslI2CReadRegister(devp->config->i2cp, devp->config->slaveaddress,
141 LSM6DSL_AD_OUTX_L_XL, buff,
142 LSM6DSL_ACC_NUMBER_OF_AXES * 2);
143
144#if LSM6DSL_SHARED_I2C
145 i2cReleaseBus(devp->config->i2cp);
146#endif /* LSM6DSL_SHARED_I2C */
147#endif /* LSM6DSL_USE_I2C */
148 if(msg == MSG_OK)
149 for(i = 0; i < LSM6DSL_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 LSM6DSLDriver* devp;
175 uint32_t i;
176 int32_t raw[LSM6DSL_ACC_NUMBER_OF_AXES];
177 msg_t msg;
178
179 osalDbgCheck((ip != NULL) && (axes != NULL));
180
181 /* Getting parent instance pointer.*/
182 devp = objGetInstance(LSM6DSLDriver*, (BaseAccelerometer*)ip);
183
184 osalDbgAssert((devp->state == LSM6DSL_READY),
185 "acc_read_cooked(), invalid state");
186
187 msg = acc_read_raw(ip, raw);
188 for(i = 0; i < LSM6DSL_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 LSM6DSLDriver* 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(LSM6DSLDriver*, (BaseAccelerometer*)ip);
215
216 osalDbgAssert((devp->state == LSM6DSL_READY),
217 "acc_set_bias(), invalid state");
218
219 for(i = 0; i < LSM6DSL_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 LSM6DSLDriver* 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(LSM6DSLDriver*, (BaseAccelerometer*)ip);
244
245 osalDbgAssert((devp->state == LSM6DSL_READY),
246 "acc_reset_bias(), invalid state");
247
248 for(i = 0; i < LSM6DSL_ACC_NUMBER_OF_AXES; i++)
249 devp->accbias[i] = LSM6DSL_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 LSM6DSLDriver* devp;
267 uint32_t i;
268 msg_t msg = MSG_OK;
269
270 /* Getting parent instance pointer.*/
271 devp = objGetInstance(LSM6DSLDriver*, (BaseAccelerometer*)ip);
272
273 osalDbgCheck((ip != NULL) && (sp != NULL));
274
275 osalDbgAssert((devp->state == LSM6DSL_READY),
276 "acc_set_sensivity(), invalid state");
277
278 for(i = 0; i < LSM6DSL_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 LSM6DSLDriver* 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(LSM6DSLDriver*, (BaseAccelerometer*)ip);
303
304 osalDbgAssert((devp->state == LSM6DSL_READY),
305 "acc_reset_sensivity(), invalid state");
306
307 if(devp->config->accfullscale == LSM6DSL_ACC_FS_2G)
308 for(i = 0; i < LSM6DSL_ACC_NUMBER_OF_AXES; i++)
309 devp->accsensitivity[i] = LSM6DSL_ACC_SENS_2G;
310 else if(devp->config->accfullscale == LSM6DSL_ACC_FS_4G)
311 for(i = 0; i < LSM6DSL_ACC_NUMBER_OF_AXES; i++)
312 devp->accsensitivity[i] = LSM6DSL_ACC_SENS_4G;
313 else if(devp->config->accfullscale == LSM6DSL_ACC_FS_8G)
314 for(i = 0; i < LSM6DSL_ACC_NUMBER_OF_AXES; i++)
315 devp->accsensitivity[i] = LSM6DSL_ACC_SENS_8G;
316 else if(devp->config->accfullscale == LSM6DSL_ACC_FS_16G)
317 for(i = 0; i < LSM6DSL_ACC_NUMBER_OF_AXES; i++)
318 devp->accsensitivity[i] = LSM6DSL_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 LSM6DSLDriver 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 LSM6DSLDriver 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(LSM6DSLDriver *devp, lsm6dsl_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 == LSM6DSL_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 == LSM6DSL_ACC_FS_2G) {
353 newfs = LSM6DSL_ACC_2G;
354 }
355 else if(fs == LSM6DSL_ACC_FS_4G) {
356 newfs = LSM6DSL_ACC_4G;
357 }
358 else if(fs == LSM6DSL_ACC_FS_8G) {
359 newfs = LSM6DSL_ACC_8G;
360 }
361 else if(fs == LSM6DSL_ACC_FS_16G) {
362 newfs = LSM6DSL_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 LSM6DSL_SHARED_I2C
375 i2cAcquireBus(devp->config->i2cp);
376 i2cStart(devp->config->i2cp,
377 devp->config->i2ccfg);
378#endif /* LSM6DSL_SHARED_I2C */
379
380 /* Updating register.*/
381 msg = lsm6dslI2CReadRegister(devp->config->i2cp,
382 devp->config->slaveaddress,
383 LSM6DSL_AD_CTRL1_XL, &buff[1], 1);
384
385#if LSM6DSL_SHARED_I2C
386 i2cReleaseBus(devp->config->i2cp);
387#endif /* LSM6DSL_SHARED_I2C */
388
389 if(msg != MSG_OK)
390 return msg;
391
392 buff[1] &= ~(LSMDSL_CTRL1_XL_FS_MASK);
393 buff[1] |= fs;
394 buff[0] = LSM6DSL_AD_CTRL1_XL;
395
396#if LSM6DSL_SHARED_I2C
397 i2cAcquireBus(devp->config->i2cp);
398 i2cStart(devp->config->i2cp, devp->config->i2ccfg);
399#endif /* LSM6DSL_SHARED_I2C */
400
401 msg = lsm6dslI2CWriteRegister(devp->config->i2cp,
402 devp->config->slaveaddress, buff, 1);
403
404#if LSM6DSL_SHARED_I2C
405 i2cReleaseBus(devp->config->i2cp);
406#endif /* LSM6DSL_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 < LSM6DSL_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 LSM6DSL_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[LSM6DSL_GYRO_NUMBER_OF_AXES]) {
450 LSM6DSLDriver* devp;
451 int16_t tmp;
452 uint8_t i, buff [2 * LSM6DSL_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(LSM6DSLDriver*, (BaseGyroscope*)ip);
459
460 osalDbgAssert((devp->state == LSM6DSL_READY),
461 "gyro_read_raw(), invalid state");
462#if LSM6DSL_USE_I2C
463 osalDbgAssert((devp->config->i2cp->state == I2C_READY),
464 "gyro_read_raw(), channel not ready");
465
466#if LSM6DSL_SHARED_I2C
467 i2cAcquireBus(devp->config->i2cp);
468 i2cStart(devp->config->i2cp,
469 devp->config->i2ccfg);
470#endif /* LSM6DSL_SHARED_I2C */
471
472 msg = lsm6dslI2CReadRegister(devp->config->i2cp, devp->config->slaveaddress,
473 LSM6DSL_AD_OUTX_L_G, buff,
474 LSM6DSL_GYRO_NUMBER_OF_AXES * 2);
475
476#if LSM6DSL_SHARED_I2C
477 i2cReleaseBus(devp->config->i2cp);
478#endif /* LSM6DSL_SHARED_I2C */
479#endif /* LSM6DSL_USE_I2C */
480
481 for(i = 0; i < LSM6DSL_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 LSM6DSLDriver* devp;
507 uint32_t i;
508 int32_t raw[LSM6DSL_GYRO_NUMBER_OF_AXES];
509 msg_t msg;
510
511 osalDbgCheck((ip != NULL) && (axes != NULL));
512
513 /* Getting parent instance pointer.*/
514 devp = objGetInstance(LSM6DSLDriver*, (BaseGyroscope*)ip);
515
516 osalDbgAssert((devp->state == LSM6DSL_READY),
517 "gyro_read_cooked(), invalid state");
518
519 msg = gyro_read_raw(ip, raw);
520 for(i = 0; i < LSM6DSL_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 LSM6DSL 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 LSM6DSL_BIAS_ACQ_TIMES
531 * and @p LSM6DSL_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 LSM6DSLDriver* devp;
540 uint32_t i, j;
541 int32_t raw[LSM6DSL_GYRO_NUMBER_OF_AXES];
542 int32_t buff[LSM6DSL_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(LSM6DSLDriver*, (BaseGyroscope*)ip);
549
550 osalDbgAssert((devp->state == LSM6DSL_READY),
551 "gyro_sample_bias(), invalid state");
552#if LSM6DSL_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 < LSM6DSL_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 < LSM6DSL_GYRO_NUMBER_OF_AXES; j++){
562 buff[j] += raw[j];
563 }
564 osalThreadSleepMicroseconds(LSM6DSL_GYRO_BIAS_SETTLING_US);
565 }
566
567 for(i = 0; i < LSM6DSL_GYRO_NUMBER_OF_AXES; i++){
568 devp->gyrobias[i] = (buff[i] / LSM6DSL_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 LSM6DSLDriver* 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(LSM6DSLDriver*, (BaseGyroscope*)ip);
595
596 osalDbgAssert((devp->state == LSM6DSL_READY),
597 "gyro_set_bias(), invalid state");
598
599 for(i = 0; i < LSM6DSL_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 LSM6DSLDriver* 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(LSM6DSLDriver*, (BaseGyroscope*)ip);
624
625 osalDbgAssert((devp->state == LSM6DSL_READY),
626 "gyro_reset_bias(), invalid state");
627
628 for(i = 0; i < LSM6DSL_GYRO_NUMBER_OF_AXES; i++)
629 devp->gyrobias[i] = LSM6DSL_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 LSM6DSLDriver* 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(LSM6DSLDriver*, (BaseGyroscope*)ip);
654
655 osalDbgAssert((devp->state == LSM6DSL_READY),
656 "gyro_set_sensivity(), invalid state");
657
658 for(i = 0; i < LSM6DSL_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 LSM6DSLDriver* 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(LSM6DSLDriver*, (BaseGyroscope*)ip);
683
684 osalDbgAssert((devp->state == LSM6DSL_READY),
685 "gyro_reset_sensivity(), invalid state");
686 if(devp->config->gyrofullscale == LSM6DSL_GYRO_FS_125DPS)
687 for(i = 0; i < LSM6DSL_GYRO_NUMBER_OF_AXES; i++)
688 devp->gyrosensitivity[i] = LSM6DSL_GYRO_SENS_125DPS;
689 else if(devp->config->gyrofullscale == LSM6DSL_GYRO_FS_250DPS)
690 for(i = 0; i < LSM6DSL_GYRO_NUMBER_OF_AXES; i++)
691 devp->gyrosensitivity[i] = LSM6DSL_GYRO_SENS_250DPS;
692 else if(devp->config->gyrofullscale == LSM6DSL_GYRO_FS_500DPS)
693 for(i = 0; i < LSM6DSL_GYRO_NUMBER_OF_AXES; i++)
694 devp->gyrosensitivity[i] = LSM6DSL_GYRO_SENS_500DPS;
695 else if(devp->config->gyrofullscale == LSM6DSL_GYRO_FS_1000DPS)
696 for(i = 0; i < LSM6DSL_GYRO_NUMBER_OF_AXES; i++)
697 devp->gyrosensitivity[i] = LSM6DSL_GYRO_SENS_1000DPS;
698 else if(devp->config->gyrofullscale == LSM6DSL_GYRO_FS_2000DPS)
699 for(i = 0; i < LSM6DSL_GYRO_NUMBER_OF_AXES; i++)
700 devp->gyrosensitivity[i] = LSM6DSL_GYRO_SENS_2000DPS;
701 else {
702 osalDbgAssert(FALSE, "gyro_reset_sensivity(), full scale issue");
703 return MSG_RESET;
704 }
705 return msg;
706}
707
708/**
709 * @brief Changes the LSM6DSLDriver gyroscope fullscale value.
710 * @note This function also rescale sensitivities and biases based on
711 * previous and next fullscale value.
712 * @note A recalibration is highly suggested after calling this function.
713 *
714 * @param[in] devp pointer to @p BaseGyroscope interface.
715 * @param[in] fs new fullscale value.
716 *
717 * @return The operation status.
718 * @retval MSG_OK if the function succeeded.
719 * @retval MSG_RESET otherwise.
720 */
721static msg_t gyro_set_full_scale(LSM6DSLDriver *devp, lsm6dsl_gyro_fs_t fs) {
722 float newfs, scale;
723 uint8_t i, buff[2];
724 msg_t msg = MSG_OK;
725
726 osalDbgCheck(devp != NULL);
727
728 osalDbgAssert((devp->state == LSM6DSL_READY),
729 "gyro_set_full_scale(), invalid state");
730#if LSM6DSL_USE_I2C
731 osalDbgAssert((devp->config->i2cp->state == I2C_READY),
732 "gyro_set_full_scale(), channel not ready");
733#endif
734
735 if(fs == LSM6DSL_GYRO_FS_125DPS) {
736 newfs = LSM6DSL_GYRO_125DPS;
737 }
738 else if(fs == LSM6DSL_GYRO_FS_250DPS) {
739 newfs = LSM6DSL_GYRO_250DPS;
740 }
741 else if(fs == LSM6DSL_GYRO_FS_500DPS) {
742 newfs = LSM6DSL_GYRO_500DPS;
743 }
744 else if(fs == LSM6DSL_GYRO_FS_1000DPS) {
745 newfs = LSM6DSL_GYRO_1000DPS;
746 }
747 else if(fs == LSM6DSL_GYRO_FS_2000DPS) {
748 newfs = LSM6DSL_GYRO_2000DPS;
749 }
750 else {
751 return MSG_RESET;
752 }
753
754 if(newfs != devp->gyrofullscale) {
755 scale = newfs / devp->gyrofullscale;
756 devp->gyrofullscale = newfs;
757
758#if LSM6DSL_USE_I2C
759#if LSM6DSL_SHARED_I2C
760 i2cAcquireBus(devp->config->i2cp);
761 i2cStart(devp->config->i2cp,
762 devp->config->i2ccfg);
763#endif /* LSM6DSL_SHARED_I2C */
764
765 /* Updating register.*/
766 msg = lsm6dslI2CReadRegister(devp->config->i2cp,
767 devp->config->slaveaddress,
768 LSM6DSL_AD_CTRL2_G, &buff[1], 1);
769
770#if LSM6DSL_SHARED_I2C
771 i2cReleaseBus(devp->config->i2cp);
772#endif /* LSM6DSL_SHARED_I2C */
773#endif /* LSM6DSL_USE_I2C */
774
775 buff[1] &= ~(LSMDSL_CTRL2_G_FS_MASK);
776 buff[1] |= fs;
777 buff[0] = LSM6DSL_AD_CTRL2_G;
778
779#if LSM6DSL_USE_I2C
780#if LSM6DSL_SHARED_I2C
781 i2cAcquireBus(devp->config->i2cp);
782 i2cStart(devp->config->i2cp,
783 devp->config->i2ccfg);
784#endif /* LSM6DSL_SHARED_I2C */
785
786 lsm6dslI2CWriteRegister(devp->config->i2cp, devp->config->slaveaddress,
787 buff, 1);
788
789#if LSM6DSL_SHARED_I2C
790 i2cReleaseBus(devp->config->i2cp);
791#endif /* LSM6DSL_SHARED_I2C */
792#endif /* LSM6DSL_USE_I2C */
793
794 /* Scaling sensitivity and bias. Re-calibration is suggested anyway. */
795 for(i = 0; i < LSM6DSL_GYRO_NUMBER_OF_AXES; i++) {
796 devp->gyrosensitivity[i] *= scale;
797 devp->gyrobias[i] *= scale;
798 }
799 }
800 return msg;
801}
802
803static const struct LSM6DSLVMT vmt_device = {
804 (size_t)0,
805 acc_set_full_scale, gyro_set_full_scale
806};
807
808static const struct BaseAccelerometerVMT vmt_accelerometer = {
809 sizeof(struct LSM6DSLVMT*),
810 acc_get_axes_number, acc_read_raw, acc_read_cooked,
811 acc_set_bias, acc_reset_bias, acc_set_sensivity, acc_reset_sensivity
812};
813
814static const struct BaseGyroscopeVMT vmt_gyroscope = {
815 sizeof(struct LSM6DSLVMT*) + sizeof(BaseAccelerometer),
816 gyro_get_axes_number, gyro_read_raw, gyro_read_cooked,
817 gyro_sample_bias, gyro_set_bias, gyro_reset_bias,
818 gyro_set_sensivity, gyro_reset_sensivity
819};
820
821/*===========================================================================*/
822/* Driver exported functions. */
823/*===========================================================================*/
824
825/**
826 * @brief Initializes an instance.
827 *
828 * @param[out] devp pointer to the @p LSM6DSLDriver object
829 *
830 * @init
831 */
832void lsm6dslObjectInit(LSM6DSLDriver *devp) {
833 devp->vmt = &vmt_device;
834 devp->acc_if.vmt = &vmt_accelerometer;
835 devp->gyro_if.vmt = &vmt_gyroscope;
836
837 devp->config = NULL;
838
839 devp->accaxes = LSM6DSL_ACC_NUMBER_OF_AXES;
840 devp->gyroaxes = LSM6DSL_GYRO_NUMBER_OF_AXES;
841
842 devp->state = LSM6DSL_STOP;
843}
844
845/**
846 * @brief Configures and activates LSM6DSL Complex Driver peripheral.
847 *
848 * @param[in] devp pointer to the @p LSM6DSLDriver object
849 * @param[in] config pointer to the @p LSM6DSLConfig object
850 *
851 * @api
852 */
853void lsm6dslStart(LSM6DSLDriver *devp, const LSM6DSLConfig *config) {
854 uint32_t i;
855 uint8_t cr[11];
856 osalDbgCheck((devp != NULL) && (config != NULL));
857
858 osalDbgAssert((devp->state == LSM6DSL_STOP) ||
859 (devp->state == LSM6DSL_READY),
860 "lsm6dslStart(), invalid state");
861
862 devp->config = config;
863
864 /* Enforcing multiple write configuration.*/
865 {
866 cr[0] = LSM6DSL_AD_CTRL3_C;
867 cr[1] = LSMDSL_CTRL3_C_IF_INC;
868 }
869#if LSM6DSL_USE_I2C
870#if LSM6DSL_SHARED_I2C
871 i2cAcquireBus(devp->config->i2cp);
872#endif /* LSM6DSL_SHARED_I2C */
873
874 i2cStart(devp->config->i2cp, devp->config->i2ccfg);
875 lsm6dslI2CWriteRegister(devp->config->i2cp, devp->config->slaveaddress,
876 cr, 1);
877
878#if LSM6DSL_SHARED_I2C
879 i2cReleaseBus(devp->config->i2cp);
880#endif /* LSM6DSL_SHARED_I2C */
881#endif /* LSM6DSL_USE_I2C */
882
883 /* Configuring all the control registers.*/
884 /* Multiple write starting address.*/
885 cr[0] = LSM6DSL_AD_CTRL1_XL;
886 /* Control register 1 configuration block.*/
887 {
888 cr[1] = devp->config->accoutdatarate |
889 devp->config->accfullscale;
890 }
891 /* Control register 2 configuration block.*/
892 {
893 cr[2] = devp->config->gyrooutdatarate |
894 devp->config->gyrofullscale;
895 }
896 /* Control register 3 configuration block.*/
897 {
898 cr[3] = LSMDSL_CTRL3_C_IF_INC;
899#if LSM6DSL_USE_ADVANCED || defined(__DOXYGEN__)
900 cr[3] |= devp->config->endianness | devp->config->blockdataupdate;
901#endif
902 }
903 /* Control register 4 configuration block.*/
904 {
905 cr[4] = 0;
906#if LSM6DSL_USE_ADVANCED || defined(__DOXYGEN__)
907 if(devp->config->gyrolowpassfilter != LSM6DSL_GYRO_LPF_DISABLED) {
908 cr[4] |= LSMDSL_CTRL4_C_LPF1_SEL_G;
909 }
910 else {
911 /* Nothing to do. */
912 }
913#endif
914 }
915 /* Control register 5 configuration block.*/
916 {
917 cr[5] = 0;
918 }
919 /* Control register 6 configuration block.*/
920 {
921 cr[6] = 0;
922#if LSM6DSL_USE_ADVANCED || defined(__DOXYGEN__)
923 cr[6] |= devp->config->acclpmode;
924
925#endif
926#if LSM6DSL_USE_ADVANCED || defined(__DOXYGEN__)
927 if(devp->config->gyrolowpassfilter != LSM6DSL_GYRO_LPF_DISABLED) {
928 cr[6] |= devp->config->gyrolowpassfilter;
929 }
930 else {
931 /* Nothing to do. */
932 }
933#endif
934 }
935 /* Control register 7 configuration block.*/
936 {
937 cr[7] = 0;
938#if LSM6DSL_USE_ADVANCED || defined(__DOXYGEN__)
939 cr[7] |= devp->config->gyrolpmode;
940
941#endif
942 }
943 /* Control register 8 configuration block.*/
944 {
945 cr[8] = 0;
946 }
947 /* Control register 9 configuration block.*/
948 {
949 cr[9] = 0;
950 }
951 /* Control register 10 configuration block.*/
952 {
953 cr[10] = 0;
954 }
955
956#if LSM6DSL_USE_I2C
957#if LSM6DSL_SHARED_I2C
958 i2cAcquireBus(devp->config->i2cp);
959 i2cStart(devp->config->i2cp, devp->config->i2ccfg);
960#endif /* LSM6DSL_SHARED_I2C */
961
962 lsm6dslI2CWriteRegister(devp->config->i2cp, devp->config->slaveaddress,
963 cr, 10);
964
965#if LSM6DSL_SHARED_I2C
966 i2cReleaseBus(devp->config->i2cp);
967#endif /* LSM6DSL_SHARED_I2C */
968#endif /* LSM6DSL_USE_I2C */
969
970 /* Storing sensitivity according to user settings */
971 if(devp->config->accfullscale == LSM6DSL_ACC_FS_2G) {
972 for(i = 0; i < LSM6DSL_ACC_NUMBER_OF_AXES; i++) {
973 if(devp->config->accsensitivity == NULL)
974 devp->accsensitivity[i] = LSM6DSL_ACC_SENS_2G;
975 else
976 devp->accsensitivity[i] = devp->config->accsensitivity[i];
977 }
978 devp->accfullscale = LSM6DSL_ACC_2G;
979 }
980 else if(devp->config->accfullscale == LSM6DSL_ACC_FS_4G) {
981 for(i = 0; i < LSM6DSL_ACC_NUMBER_OF_AXES; i++) {
982 if(devp->config->accsensitivity == NULL)
983 devp->accsensitivity[i] = LSM6DSL_ACC_SENS_4G;
984 else
985 devp->accsensitivity[i] = devp->config->accsensitivity[i];
986 }
987 devp->accfullscale = LSM6DSL_ACC_4G;
988 }
989 else if(devp->config->accfullscale == LSM6DSL_ACC_FS_8G) {
990 for(i = 0; i < LSM6DSL_ACC_NUMBER_OF_AXES; i++) {
991 if(devp->config->accsensitivity == NULL)
992 devp->accsensitivity[i] = LSM6DSL_ACC_SENS_8G;
993 else
994 devp->accsensitivity[i] = devp->config->accsensitivity[i];
995 }
996 devp->accfullscale = LSM6DSL_ACC_8G;
997 }
998 else if(devp->config->accfullscale == LSM6DSL_ACC_FS_16G) {
999 for(i = 0; i < LSM6DSL_ACC_NUMBER_OF_AXES; i++) {
1000 if(devp->config->accsensitivity == NULL)
1001 devp->accsensitivity[i] = LSM6DSL_ACC_SENS_16G;
1002 else
1003 devp->accsensitivity[i] = devp->config->accsensitivity[i];
1004 }
1005 devp->accfullscale = LSM6DSL_ACC_16G;
1006 }
1007 else
1008 osalDbgAssert(FALSE, "lsm6dslStart(), accelerometer full scale issue");
1009
1010 /* Storing bias information */
1011 if(devp->config->accbias != NULL)
1012 for(i = 0; i < LSM6DSL_ACC_NUMBER_OF_AXES; i++)
1013 devp->accbias[i] = devp->config->accbias[i];
1014 else
1015 for(i = 0; i < LSM6DSL_ACC_NUMBER_OF_AXES; i++)
1016 devp->accbias[i] = LSM6DSL_ACC_BIAS;
1017
1018 if(devp->config->gyrofullscale == LSM6DSL_GYRO_FS_125DPS) {
1019 for(i = 0; i < LSM6DSL_GYRO_NUMBER_OF_AXES; i++) {
1020 if(devp->config->gyrosensitivity == NULL)
1021 devp->gyrosensitivity[i] = LSM6DSL_GYRO_SENS_125DPS;
1022 else
1023 devp->gyrosensitivity[i] = devp->config->gyrosensitivity[i];
1024 }
1025 devp->gyrofullscale = LSM6DSL_GYRO_125DPS;
1026 }
1027 else if(devp->config->gyrofullscale == LSM6DSL_GYRO_FS_250DPS) {
1028 for(i = 0; i < LSM6DSL_GYRO_NUMBER_OF_AXES; i++) {
1029 if(devp->config->gyrosensitivity == NULL)
1030 devp->gyrosensitivity[i] = LSM6DSL_GYRO_SENS_250DPS;
1031 else
1032 devp->gyrosensitivity[i] = devp->config->gyrosensitivity[i];
1033 }
1034 devp->gyrofullscale = LSM6DSL_GYRO_250DPS;
1035 }
1036 else if(devp->config->gyrofullscale == LSM6DSL_GYRO_FS_500DPS) {
1037 for(i = 0; i < LSM6DSL_GYRO_NUMBER_OF_AXES; i++) {
1038 if(devp->config->gyrosensitivity == NULL)
1039 devp->gyrosensitivity[i] = LSM6DSL_GYRO_SENS_500DPS;
1040 else
1041 devp->gyrosensitivity[i] = devp->config->gyrosensitivity[i];
1042 }
1043 devp->gyrofullscale = LSM6DSL_GYRO_500DPS;
1044 }
1045 else if(devp->config->gyrofullscale == LSM6DSL_GYRO_FS_1000DPS) {
1046 for(i = 0; i < LSM6DSL_GYRO_NUMBER_OF_AXES; i++) {
1047 if(devp->config->gyrosensitivity == NULL)
1048 devp->gyrosensitivity[i] = LSM6DSL_GYRO_SENS_1000DPS;
1049 else
1050 devp->gyrosensitivity[i] = devp->config->gyrosensitivity[i];
1051 }
1052 devp->gyrofullscale = LSM6DSL_GYRO_1000DPS;
1053 }
1054 else if(devp->config->gyrofullscale == LSM6DSL_GYRO_FS_2000DPS) {
1055 for(i = 0; i < LSM6DSL_GYRO_NUMBER_OF_AXES; i++) {
1056 if(devp->config->gyrosensitivity == NULL)
1057 devp->gyrosensitivity[i] = LSM6DSL_GYRO_SENS_2000DPS;
1058 else
1059 devp->gyrosensitivity[i] = devp->config->gyrosensitivity[i];
1060 }
1061 devp->gyrofullscale = LSM6DSL_GYRO_2000DPS;
1062 }
1063 else
1064 osalDbgAssert(FALSE, "lsm6dslStart(), gyroscope full scale issue");
1065
1066 /* Storing bias information */
1067 if(devp->config->gyrobias != NULL)
1068 for(i = 0; i < LSM6DSL_GYRO_NUMBER_OF_AXES; i++)
1069 devp->gyrobias[i] = devp->config->gyrobias[i];
1070 else
1071 for(i = 0; i < LSM6DSL_GYRO_NUMBER_OF_AXES; i++)
1072 devp->gyrobias[i] = LSM6DSL_GYRO_BIAS;
1073
1074 /* This is the MEMS transient recovery time */
1075 osalThreadSleepMilliseconds(5);
1076
1077 devp->state = LSM6DSL_READY;
1078}
1079
1080/**
1081 * @brief Deactivates the LSM6DSL Complex Driver peripheral.
1082 *
1083 * @param[in] devp pointer to the @p LSM6DSLDriver object
1084 *
1085 * @api
1086 */
1087void lsm6dslStop(LSM6DSLDriver *devp) {
1088 uint8_t cr[2];
1089
1090 osalDbgCheck(devp != NULL);
1091
1092 osalDbgAssert((devp->state == LSM6DSL_STOP) || (devp->state == LSM6DSL_READY),
1093 "lsm6dslStop(), invalid state");
1094
1095 if (devp->state == LSM6DSL_READY) {
1096#if LSM6DSL_USE_I2C
1097#if LSM6DSL_SHARED_I2C
1098 i2cAcquireBus(devp->config->i2cp);
1099 i2cStart(devp->config->i2cp, devp->config->i2ccfg);
1100#endif /* LSM6DSL_SHARED_I2C */
1101
1102
1103 cr[0] = LSM6DSL_AD_CTRL1_XL;
1104 /* Disabling accelerometer.*/
1105 cr[1] = LSM6DSL_ACC_ODR_PD;
1106 /* Disabling gyroscope.*/
1107 cr[2] = LSM6DSL_GYRO_ODR_PD;
1108 lsm6dslI2CWriteRegister(devp->config->i2cp, devp->config->slaveaddress,
1109 cr, 2);
1110
1111 i2cStop(devp->config->i2cp);
1112#if LSM6DSL_SHARED_I2C
1113 i2cReleaseBus(devp->config->i2cp);
1114#endif /* LSM6DSL_SHARED_I2C */
1115#endif /* LSM6DSL_USE_I2C */
1116 }
1117 devp->state = LSM6DSL_STOP;
1118}
1119/** @} */
generated by cgit v1.2.3 (git 2.25.1) at 2025-08-30 17:04:13 +0100