aboutsummaryrefslogtreecommitdiff
path: root/lib/chibios/os/ex/devices/ST/lsm303agr.c
diff options
context:
space:
mode:
Diffstat (limited to 'lib/chibios/os/ex/devices/ST/lsm303agr.c')
-rw-r--r--lib/chibios/os/ex/devices/ST/lsm303agr.c906
1 files changed, 906 insertions, 0 deletions
diff --git a/lib/chibios/os/ex/devices/ST/lsm303agr.c b/lib/chibios/os/ex/devices/ST/lsm303agr.c
new file mode 100644
index 000000000..ba75ecd6c
--- /dev/null
+++ b/lib/chibios/os/ex/devices/ST/lsm303agr.c
@@ -0,0 +1,906 @@
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 lsm303agr.c
23 * @brief LSM303AGR MEMS interface module code.
24 *
25 * @addtogroup LSM303AGR
26 * @ingroup EX_ST
27 * @{
28 */
29
30#include "hal.h"
31#include "lsm303agr.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 * @brief Accelerometer and Compass Slave Address.
47 */
48typedef enum {
49 LSM303AGR_SAD_ACC = 0x19, /**< SAD for accelerometer. */
50 LSM303AGR_SAD_COMP = 0x1E /**< SAD for compass. */
51} lsm303agr_sad_t;
52
53/*===========================================================================*/
54/* Driver local functions. */
55/*===========================================================================*/
56
57/**
58 * @brief Reads registers value using I2C.
59 * @pre The I2C interface must be initialized and the driver started.
60 * @note IF_ADD_INC bit must be 1 in CTRL_REG8.
61 *
62 * @param[in] i2cp pointer to the I2C interface.
63 * @param[in] sad slave address without R bit.
64 * @param[in] reg first sub-register address.
65 * @param[in] rxbuf receiving buffer.
66 * @param[in] n size of rxbuf.
67 * @return the operation status.
68 */
69static msg_t lsm303agrI2CReadRegister(I2CDriver *i2cp, lsm303agr_sad_t sad,
70 uint8_t reg, uint8_t *rxbuf, size_t n) {
71
72 uint8_t txbuf = reg | LSM303AGR_MS;
73 return i2cMasterTransmitTimeout(i2cp, sad, &txbuf, 1, rxbuf, n,
74 TIME_INFINITE);
75}
76
77/**
78 * @brief Writes a value into a register using I2C.
79 * @pre The I2C interface must be initialized and the driver started.
80 *
81 * @param[in] i2cp pointer to the I2C interface.
82 * @param[in] sad slave address without R bit.
83 * @param[in] txbuf buffer containing sub-address value in first position
84 * and values to write.
85 * @param[in] n size of txbuf less one (not considering the first
86 * element).
87 * @return the operation status.
88 */
89static msg_t lsm303agrI2CWriteRegister(I2CDriver *i2cp, lsm303agr_sad_t sad,
90 uint8_t *txbuf, size_t n) {
91 if (n != 1)
92 *txbuf |= LSM303AGR_MS;
93 return i2cMasterTransmitTimeout(i2cp, sad, txbuf, n + 1, NULL, 0,
94 TIME_INFINITE);
95}
96
97/**
98 * @brief Return the number of axes of the BaseAccelerometer.
99 *
100 * @param[in] ip pointer to @p BaseAccelerometer interface.
101 *
102 * @return the number of axes.
103 */
104static size_t acc_get_axes_number(void *ip) {
105 (void)ip;
106
107 return LSM303AGR_ACC_NUMBER_OF_AXES;
108}
109
110/**
111 * @brief Retrieves raw data from the BaseAccelerometer.
112 * @note This data is retrieved from MEMS register without any algebraical
113 * manipulation.
114 * @note The axes array must be at least the same size of the
115 * BaseAccelerometer axes number.
116 *
117 * @param[in] ip pointer to @p BaseAccelerometer interface.
118 * @param[out] axes a buffer which would be filled with raw data.
119 *
120 * @return The operation status.
121 * @retval MSG_OK if the function succeeded.
122 * @retval MSG_RESET if one or more I2C errors occurred, the errors can
123 * be retrieved using @p i2cGetErrors().
124 * @retval MSG_TIMEOUT if a timeout occurred before operation end.
125 */
126static msg_t acc_read_raw(void *ip, int32_t axes[]) {
127 LSM303AGRDriver* devp;
128 uint8_t buff [LSM303AGR_ACC_NUMBER_OF_AXES * 2], i;
129 int16_t tmp;
130 msg_t msg;
131
132 osalDbgCheck((ip != NULL) && (axes != NULL));
133
134 /* Getting parent instance pointer.*/
135 devp = objGetInstance(LSM303AGRDriver*, (BaseAccelerometer*)ip);
136
137 osalDbgAssert((devp->state == LSM303AGR_READY),
138 "acc_read_raw(), invalid state");
139 osalDbgAssert((devp->config->i2cp->state == I2C_READY),
140 "acc_read_raw(), channel not ready");
141
142#if LSM303AGR_SHARED_I2C
143 i2cAcquireBus(devp->config->i2cp);
144 i2cStart(devp->config->i2cp,
145 devp->config->i2ccfg);
146#endif /* LSM303AGR_SHARED_I2C */
147
148 msg = lsm303agrI2CReadRegister(devp->config->i2cp, LSM303AGR_SAD_ACC,
149 LSM303AGR_AD_OUT_X_L_A, buff,
150 LSM303AGR_ACC_NUMBER_OF_AXES * 2);
151
152#if LSM303AGR_SHARED_I2C
153 i2cReleaseBus(devp->config->i2cp);
154#endif /* LSM303AGR_SHARED_I2C */
155
156 if(msg == MSG_OK)
157 for(i = 0; i < LSM303AGR_ACC_NUMBER_OF_AXES; i++) {
158 tmp = buff[2 * i] + (buff[2 * i + 1] << 8);
159 axes[i] = (int32_t)tmp;
160 }
161 return msg;
162}
163
164/**
165 * @brief Retrieves cooked data from the BaseAccelerometer.
166 * @note This data is manipulated according to the formula
167 * cooked = (raw * sensitivity) - bias.
168 * @note Final data is expressed as milli-G.
169 * @note The axes array must be at least the same size of the
170 * BaseAccelerometer axes number.
171 *
172 * @param[in] ip pointer to @p BaseAccelerometer interface.
173 * @param[out] axes a buffer which would be filled with cooked data.
174 *
175 * @return The operation status.
176 * @retval MSG_OK if the function succeeded.
177 * @retval MSG_RESET if one or more I2C errors occurred, the errors can
178 * be retrieved using @p i2cGetErrors().
179 * @retval MSG_TIMEOUT if a timeout occurred before operation end.
180 */
181static msg_t acc_read_cooked(void *ip, float axes[]) {
182 LSM303AGRDriver* devp;
183 uint32_t i;
184 int32_t raw[LSM303AGR_ACC_NUMBER_OF_AXES];
185 msg_t msg;
186
187 osalDbgCheck((ip != NULL) && (axes != NULL));
188
189 /* Getting parent instance pointer.*/
190 devp = objGetInstance(LSM303AGRDriver*, (BaseAccelerometer*)ip);
191
192 osalDbgAssert((devp->state == LSM303AGR_READY),
193 "acc_read_cooked(), invalid state");
194
195 msg = acc_read_raw(ip, raw);
196 for(i = 0; i < LSM303AGR_ACC_NUMBER_OF_AXES; i++) {
197 axes[i] = (raw[i] * devp->accsensitivity[i]) - devp->accbias[i];
198 }
199 return msg;
200}
201
202/**
203 * @brief Set bias values for the BaseAccelerometer.
204 * @note Bias must be expressed as milli-G.
205 * @note The bias buffer must be at least the same size of the
206 * BaseAccelerometer axes number.
207 *
208 * @param[in] ip pointer to @p BaseAccelerometer interface.
209 * @param[in] bp a buffer which contains biases.
210 *
211 * @return The operation status.
212 * @retval MSG_OK if the function succeeded.
213 */
214static msg_t acc_set_bias(void *ip, float *bp) {
215 LSM303AGRDriver* devp;
216 uint32_t i;
217 msg_t msg = MSG_OK;
218
219 osalDbgCheck((ip != NULL) && (bp != NULL));
220
221 /* Getting parent instance pointer.*/
222 devp = objGetInstance(LSM303AGRDriver*, (BaseAccelerometer*)ip);
223
224 osalDbgAssert((devp->state == LSM303AGR_READY),
225 "acc_set_bias(), invalid state");
226
227 for(i = 0; i < LSM303AGR_ACC_NUMBER_OF_AXES; i++) {
228 devp->accbias[i] = bp[i];
229 }
230 return msg;
231}
232
233/**
234 * @brief Reset bias values for the BaseAccelerometer.
235 * @note Default biases value are obtained from device datasheet when
236 * available otherwise they are considered zero.
237 *
238 * @param[in] ip pointer to @p BaseAccelerometer interface.
239 *
240 * @return The operation status.
241 * @retval MSG_OK if the function succeeded.
242 */
243static msg_t acc_reset_bias(void *ip) {
244 LSM303AGRDriver* devp;
245 uint32_t i;
246 msg_t msg = MSG_OK;
247
248 osalDbgCheck(ip != NULL);
249
250 /* Getting parent instance pointer.*/
251 devp = objGetInstance(LSM303AGRDriver*, (BaseAccelerometer*)ip);
252
253 osalDbgAssert((devp->state == LSM303AGR_READY),
254 "acc_reset_bias(), invalid state");
255
256 for(i = 0; i < LSM303AGR_ACC_NUMBER_OF_AXES; i++)
257 devp->accbias[i] = LSM303AGR_ACC_BIAS;
258 return msg;
259}
260
261/**
262 * @brief Set sensitivity values for the BaseAccelerometer.
263 * @note Sensitivity must be expressed as milli-G/LSB.
264 * @note The sensitivity buffer must be at least the same size of the
265 * BaseAccelerometer axes number.
266 *
267 * @param[in] ip pointer to @p BaseAccelerometer interface.
268 * @param[in] sp a buffer which contains sensitivities.
269 *
270 * @return The operation status.
271 * @retval MSG_OK if the function succeeded.
272 */
273static msg_t acc_set_sensivity(void *ip, float *sp) {
274 LSM303AGRDriver* devp;
275 uint32_t i;
276 msg_t msg = MSG_OK;
277
278 /* Getting parent instance pointer.*/
279 devp = objGetInstance(LSM303AGRDriver*, (BaseAccelerometer*)ip);
280
281 osalDbgCheck((ip != NULL) && (sp != NULL));
282
283 osalDbgAssert((devp->state == LSM303AGR_READY),
284 "acc_set_sensivity(), invalid state");
285
286 for(i = 0; i < LSM303AGR_ACC_NUMBER_OF_AXES; i++) {
287 devp->accsensitivity[i] = sp[i];
288 }
289 return msg;
290}
291
292/**
293 * @brief Reset sensitivity values for the BaseAccelerometer.
294 * @note Default sensitivities value are obtained from device datasheet.
295 *
296 * @param[in] ip pointer to @p BaseAccelerometer interface.
297 *
298 * @return The operation status.
299 * @retval MSG_OK if the function succeeded.
300 * @retval MSG_RESET otherwise.
301 */
302static msg_t acc_reset_sensivity(void *ip) {
303 LSM303AGRDriver* devp;
304 uint32_t i;
305 msg_t msg = MSG_OK;
306
307 osalDbgCheck(ip != NULL);
308
309 /* Getting parent instance pointer.*/
310 devp = objGetInstance(LSM303AGRDriver*, (BaseAccelerometer*)ip);
311
312 osalDbgAssert((devp->state == LSM303AGR_READY),
313 "acc_reset_sensivity(), invalid state");
314
315 if(devp->config->accfullscale == LSM303AGR_ACC_FS_2G) {
316 for(i = 0; i < LSM303AGR_ACC_NUMBER_OF_AXES; i++) {
317 devp->accsensitivity[i] = LSM303AGR_ACC_SENS_2G;
318 }
319 }
320 else if(devp->config->accfullscale == LSM303AGR_ACC_FS_4G) {
321 for(i = 0; i < LSM303AGR_ACC_NUMBER_OF_AXES; i++) {
322 devp->accsensitivity[i] = LSM303AGR_ACC_SENS_4G;
323 }
324 }
325 else if(devp->config->accfullscale == LSM303AGR_ACC_FS_8G) {
326 for(i = 0; i < LSM303AGR_ACC_NUMBER_OF_AXES; i++) {
327 devp->accsensitivity[i] = LSM303AGR_ACC_SENS_8G;
328 }
329 }
330 else if(devp->config->accfullscale == LSM303AGR_ACC_FS_16G) {
331 for(i = 0; i < LSM303AGR_ACC_NUMBER_OF_AXES; i++) {
332 devp->accsensitivity[i] = LSM303AGR_ACC_SENS_16G;
333 }
334 }
335 else {
336 osalDbgAssert(FALSE, "reset_sensivity(), accelerometer full scale issue");
337 msg = MSG_RESET;
338 }
339 return msg;
340}
341
342/**
343 * @brief Changes the LSM303AGRDriver accelerometer fullscale value.
344 * @note This function also rescale sensitivities and biases based on
345 * previous and next fullscale value.
346 * @note A recalibration is highly suggested after calling this function.
347 *
348 * @param[in] devp pointer to @p LSM303AGRDriver interface.
349 * @param[in] fs new fullscale value.
350 *
351 * @return The operation status.
352 * @retval MSG_OK if the function succeeded.
353 * @retval MSG_RESET otherwise.
354 */
355static msg_t acc_set_full_scale(LSM303AGRDriver *devp,
356 lsm303agr_acc_fs_t fs) {
357 float newfs, scale;
358 uint8_t i, buff[2];
359 msg_t msg;
360
361 osalDbgCheck(devp != NULL);
362
363 osalDbgAssert((devp->state == LSM303AGR_READY),
364 "acc_set_full_scale(), invalid state");
365 osalDbgAssert((devp->config->i2cp->state == I2C_READY),
366 "acc_set_full_scale(), channel not ready");
367
368 /* Computing new fullscale value.*/
369 if(fs == LSM303AGR_ACC_FS_2G) {
370 newfs = LSM303AGR_ACC_2G;
371 }
372 else if(fs == LSM303AGR_ACC_FS_4G) {
373 newfs = LSM303AGR_ACC_4G;
374 }
375 else if(fs == LSM303AGR_ACC_FS_8G) {
376 newfs = LSM303AGR_ACC_8G;
377 }
378 else if(fs == LSM303AGR_ACC_FS_16G) {
379 newfs = LSM303AGR_ACC_16G;
380 }
381 else {
382 msg = MSG_RESET;
383 return msg;
384 }
385
386 if(newfs != devp->accfullscale) {
387 /* Computing scale value.*/
388 scale = newfs / devp->accfullscale;
389 devp->accfullscale = newfs;
390
391#if LSM303AGR_SHARED_I2C
392 i2cAcquireBus(devp->config->i2cp);
393 i2cStart(devp->config->i2cp,
394 devp->config->i2ccfg);
395#endif /* LSM303AGR_SHARED_I2C */
396
397 /* Updating register.*/
398 msg = lsm303agrI2CReadRegister(devp->config->i2cp,
399 LSM303AGR_SAD_ACC,
400 LSM303AGR_AD_CTRL_REG4_A,
401 &buff[1], 1);
402
403#if LSM303AGR_SHARED_I2C
404 i2cReleaseBus(devp->config->i2cp);
405#endif /* LSM303AGR_SHARED_I2C */
406
407 if(msg != MSG_OK)
408 return msg;
409
410 buff[1] &= ~(LSM303AGR_CTRL_REG4_A_FS_MASK);
411 buff[1] |= fs;
412 buff[0] = LSM303AGR_AD_CTRL_REG4_A;
413
414#if LSM303AGR_SHARED_I2C
415 i2cAcquireBus(devp->config->i2cp);
416 i2cStart(devp->config->i2cp, devp->config->i2ccfg);
417#endif /* LSM303AGR_SHARED_I2C */
418
419 msg = lsm303agrI2CWriteRegister(devp->config->i2cp,
420 LSM303AGR_SAD_ACC, buff, 1);
421
422#if LSM303AGR_SHARED_I2C
423 i2cReleaseBus(devp->config->i2cp);
424#endif /* LSM303AGR_SHARED_I2C */
425
426 if(msg != MSG_OK)
427 return msg;
428
429 /* Scaling sensitivity and bias. Re-calibration is suggested anyway.*/
430 for(i = 0; i < LSM303AGR_ACC_NUMBER_OF_AXES; i++) {
431 devp->accsensitivity[i] *= scale;
432 devp->accbias[i] *= scale;
433 }
434 }
435 return msg;
436}
437
438/**
439 * @brief Return the number of axes of the BaseCompass.
440 *
441 * @param[in] ip pointer to @p BaseCompass interface
442 *
443 * @return the number of axes.
444 */
445static size_t comp_get_axes_number(void *ip) {
446
447 osalDbgCheck(ip != NULL);
448 return LSM303AGR_COMP_NUMBER_OF_AXES;
449}
450
451/**
452 * @brief Retrieves raw data from the BaseCompass.
453 * @note This data is retrieved from MEMS register without any algebraical
454 * manipulation.
455 * @note The axes array must be at least the same size of the
456 * BaseCompass axes number.
457 *
458 * @param[in] ip pointer to @p BaseCompass interface.
459 * @param[out] axes a buffer which would be filled with raw data.
460 *
461 * @return The operation status.
462 * @retval MSG_OK if the function succeeded.
463 * @retval MSG_RESET if one or more I2C errors occurred, the errors can
464 * be retrieved using @p i2cGetErrors().
465 * @retval MSG_TIMEOUT if a timeout occurred before operation end.
466 */
467static msg_t comp_read_raw(void *ip, int32_t axes[]) {
468 LSM303AGRDriver* devp;
469 uint8_t buff [LSM303AGR_COMP_NUMBER_OF_AXES * 2], i;
470 int16_t tmp;
471 msg_t msg;
472
473 osalDbgCheck((ip != NULL) && (axes != NULL));
474
475 /* Getting parent instance pointer.*/
476 devp = objGetInstance(LSM303AGRDriver*, (BaseCompass*)ip);
477
478 osalDbgAssert((devp->state == LSM303AGR_READY),
479 "comp_read_raw(), invalid state");
480 osalDbgAssert((devp->config->i2cp->state == I2C_READY),
481 "comp_read_raw(), channel not ready");
482
483#if LSM303AGR_SHARED_I2C
484 i2cAcquireBus(devp->config->i2cp);
485 i2cStart(devp->config->i2cp,
486 devp->config->i2ccfg);
487#endif /* LSM303AGR_SHARED_I2C */
488 msg = lsm303agrI2CReadRegister(devp->config->i2cp, LSM303AGR_SAD_COMP,
489 LSM303AGR_AD_OUTX_L_REG_M, buff,
490 LSM303AGR_COMP_NUMBER_OF_AXES * 2);
491
492#if LSM303AGR_SHARED_I2C
493 i2cReleaseBus(devp->config->i2cp);
494#endif /* LSM303AGR_SHARED_I2C */
495
496 if(msg == MSG_OK)
497 for(i = 0; i < LSM303AGR_COMP_NUMBER_OF_AXES; i++) {
498 tmp = buff[2 * i] + (buff[2 * i + 1] << 8);
499 axes[i] = (int32_t)tmp;
500 }
501 return msg;
502}
503
504/**
505 * @brief Retrieves cooked data from the BaseCompass.
506 * @note This data is manipulated according to the formula
507 * cooked = (raw * sensitivity) - bias.
508 * @note Final data is expressed as G.
509 * @note The axes array must be at least the same size of the
510 * BaseCompass axes number.
511 *
512 * @param[in] ip pointer to @p BaseCompass interface.
513 * @param[out] axes a buffer which would be filled with cooked data.
514 *
515 * @return The operation status.
516 * @retval MSG_OK if the function succeeded.
517 * @retval MSG_RESET if one or more I2C errors occurred, the errors can
518 * be retrieved using @p i2cGetErrors().
519 * @retval MSG_TIMEOUT if a timeout occurred before operation end.
520 */
521static msg_t comp_read_cooked(void *ip, float axes[]) {
522 LSM303AGRDriver* devp;
523 uint32_t i;
524 int32_t raw[LSM303AGR_COMP_NUMBER_OF_AXES];
525 msg_t msg;
526
527 osalDbgCheck((ip != NULL) && (axes != NULL));
528
529
530 /* Getting parent instance pointer.*/
531 devp = objGetInstance(LSM303AGRDriver*, (BaseCompass*)ip);
532
533 osalDbgAssert((devp->state == LSM303AGR_READY),
534 "comp_read_cooked(), invalid state");
535
536 msg = comp_read_raw(ip, raw);
537 for(i = 0; i < LSM303AGR_COMP_NUMBER_OF_AXES ; i++) {
538 axes[i] = (raw[i] * devp->compsensitivity[i]) - devp->compbias[i];
539 }
540 return msg;
541}
542
543/**
544 * @brief Set bias values for the BaseCompass.
545 * @note Bias must be expressed as G.
546 * @note The bias buffer must be at least the same size of the
547 * BaseCompass axes number.
548 *
549 * @param[in] ip pointer to @p BaseCompass interface.
550 * @param[in] bp a buffer which contains biases.
551 *
552 * @return The operation status.
553 * @retval MSG_OK if the function succeeded.
554 */
555static msg_t comp_set_bias(void *ip, float *bp) {
556 LSM303AGRDriver* devp;
557 uint32_t i;
558 msg_t msg = MSG_OK;
559
560 osalDbgCheck((ip != NULL) && (bp != NULL));
561
562 /* Getting parent instance pointer.*/
563 devp = objGetInstance(LSM303AGRDriver*, (BaseCompass*)ip);
564
565 osalDbgAssert((devp->state == LSM303AGR_READY),
566 "comp_set_bias(), invalid state");
567
568 for(i = 0; i < LSM303AGR_COMP_NUMBER_OF_AXES; i++) {
569 devp->compbias[i] = bp[i];
570 }
571 return msg;
572}
573
574/**
575 * @brief Reset bias values for the BaseCompass.
576 * @note Default biases value are obtained from device datasheet when
577 * available otherwise they are considered zero.
578 *
579 * @param[in] ip pointer to @p BaseCompass interface.
580 *
581 * @return The operation status.
582 * @retval MSG_OK if the function succeeded.
583 */
584static msg_t comp_reset_bias(void *ip) {
585 LSM303AGRDriver* devp;
586 uint32_t i;
587 msg_t msg = MSG_OK;
588
589 osalDbgCheck(ip != NULL);
590
591 /* Getting parent instance pointer.*/
592 devp = objGetInstance(LSM303AGRDriver*, (BaseCompass*)ip);
593
594 osalDbgAssert((devp->state == LSM303AGR_READY),
595 "comp_reset_bias(), invalid state");
596
597 for(i = 0; i < LSM303AGR_COMP_NUMBER_OF_AXES; i++)
598 devp->compbias[i] = LSM303AGR_COMP_BIAS;
599 return msg;
600}
601
602/**
603 * @brief Set sensitivity values for the BaseCompass.
604 * @note Sensitivity must be expressed as G/LSB.
605 * @note The sensitivity buffer must be at least the same size of the
606 * BaseCompass axes number.
607 *
608 * @param[in] ip pointer to @p BaseCompass interface.
609 * @param[in] sp a buffer which contains sensitivities.
610 *
611 * @return The operation status.
612 * @retval MSG_OK if the function succeeded.
613 */
614static msg_t comp_set_sensivity(void *ip, float *sp) {
615 LSM303AGRDriver* devp;
616 uint32_t i;
617 msg_t msg = MSG_OK;
618
619 /* Getting parent instance pointer.*/
620 devp = objGetInstance(LSM303AGRDriver*, (BaseCompass*)ip);
621
622 osalDbgCheck((ip != NULL) && (sp != NULL));
623
624 osalDbgAssert((devp->state == LSM303AGR_READY),
625 "comp_set_sensivity(), invalid state");
626
627 for(i = 0; i < LSM303AGR_COMP_NUMBER_OF_AXES; i++) {
628 devp->compsensitivity[i] = sp[i];
629 }
630 return msg;
631}
632
633/**
634 * @brief Reset sensitivity values for the BaseCompass.
635 * @note Default sensitivities value are obtained from device datasheet.
636 *
637 * @param[in] ip pointer to @p BaseCompass interface.
638 *
639 * @return The operation status.
640 * @retval MSG_OK if the function succeeded.
641 * @retval MSG_RESET otherwise.
642 */
643static msg_t comp_reset_sensivity(void *ip) {
644 LSM303AGRDriver* devp;
645 uint32_t i;
646 msg_t msg = MSG_OK;
647
648 osalDbgCheck(ip != NULL);
649
650 /* Getting parent instance pointer.*/
651 devp = objGetInstance(LSM303AGRDriver*, (BaseCompass*)ip);
652
653 osalDbgAssert((devp->state == LSM303AGR_READY),
654 "comp_reset_sensivity(), invalid state");
655
656 for(i = 0; i < LSM303AGR_COMP_NUMBER_OF_AXES; i++)
657 devp->compsensitivity[i] = LSM303AGR_COMP_SENS_50GA;
658
659 return msg;
660}
661
662static const struct LSM303AGRVMT vmt_device = {
663 (size_t)0,
664 acc_set_full_scale
665};
666
667static const struct BaseAccelerometerVMT vmt_accelerometer = {
668 sizeof(struct LSM303AGRVMT*),
669 acc_get_axes_number, acc_read_raw, acc_read_cooked,
670 acc_set_bias, acc_reset_bias, acc_set_sensivity, acc_reset_sensivity
671};
672
673static const struct BaseCompassVMT vmt_compass = {
674 sizeof(struct LSM303AGRVMT*) + sizeof(BaseAccelerometer),
675 comp_get_axes_number, comp_read_raw, comp_read_cooked,
676 comp_set_bias, comp_reset_bias, comp_set_sensivity, comp_reset_sensivity
677};
678
679/*===========================================================================*/
680/* Driver exported functions. */
681/*===========================================================================*/
682
683/**
684 * @brief Initializes an instance.
685 *
686 * @param[out] devp pointer to the @p LSM303AGRDriver object
687 *
688 * @init
689 */
690void lsm303agrObjectInit(LSM303AGRDriver *devp) {
691 devp->vmt = &vmt_device;
692 devp->acc_if.vmt = &vmt_accelerometer;
693 devp->comp_if.vmt = &vmt_compass;
694
695 devp->config = NULL;
696
697 devp->accaxes = LSM303AGR_ACC_NUMBER_OF_AXES;
698 devp->compaxes = LSM303AGR_COMP_NUMBER_OF_AXES;
699
700 devp->state = LSM303AGR_STOP;
701}
702
703/**
704 * @brief Configures and activates LSM303AGR Complex Driver peripheral.
705 *
706 * @param[in] devp pointer to the @p LSM303AGRDriver object
707 * @param[in] config pointer to the @p LSM303AGRConfig object
708 *
709 * @api
710 */
711void lsm303agrStart(LSM303AGRDriver *devp, const LSM303AGRConfig *config) {
712 uint32_t i;
713 uint8_t cr[6];
714 osalDbgCheck((devp != NULL) && (config != NULL));
715
716 osalDbgAssert((devp->state == LSM303AGR_STOP) ||
717 (devp->state == LSM303AGR_READY),
718 "lsm303agrStart(), invalid state");
719
720 devp->config = config;
721
722 /* Configuring Accelerometer subsystem.*/
723
724 /* Multiple write starting address.*/
725 cr[0] = LSM303AGR_AD_CTRL_REG1_A;
726
727 /* Control register 1 configuration block.*/
728 {
729 cr[1] = LSM303AGR_ACC_AE_XYZ | devp->config->accoutdatarate;
730#if LSM303AGR_USE_ADVANCED || defined(__DOXYGEN__)
731 if(devp->config->accmode == LSM303AGR_ACC_MODE_LPOW)
732 cr[1] |= LSM303AGR_CTRL_REG1_A_LPEN;
733#endif
734 }
735
736 /* Control register 2 configuration block.*/
737 {
738 cr[2] = 0;
739 }
740
741 /* Control register 3 configuration block.*/
742 {
743 cr[3] = 0;
744 }
745
746 /* Control register 4 configuration block.*/
747 {
748 cr[4] = devp->config->accfullscale;
749#if LSM303AGR_USE_ADVANCED || defined(__DOXYGEN__)
750 cr[4] |= devp->config->accendianess |
751 devp->config->accblockdataupdate;
752 if(devp->config->accmode == LSM303AGR_ACC_MODE_HRES)
753 cr[4] |= LSM303AGR_CTRL_REG4_A_HR;
754#endif
755 }
756
757 /* Storing sensitivity according to user settings */
758 if(devp->config->accfullscale == LSM303AGR_ACC_FS_2G) {
759 devp->accfullscale = LSM303AGR_ACC_2G;
760 for(i = 0; i < LSM303AGR_ACC_NUMBER_OF_AXES; i++) {
761 if(devp->config->accsensitivity == NULL)
762 devp->accsensitivity[i] = LSM303AGR_ACC_SENS_2G;
763 else
764 devp->accsensitivity[i] = devp->config->accsensitivity[i];
765 }
766 }
767 else if(devp->config->accfullscale == LSM303AGR_ACC_FS_4G) {
768 devp->accfullscale = LSM303AGR_ACC_4G;
769 for(i = 0; i < LSM303AGR_ACC_NUMBER_OF_AXES; i++) {
770 if(devp->config->accsensitivity == NULL)
771 devp->accsensitivity[i] = LSM303AGR_ACC_SENS_4G;
772 else
773 devp->accsensitivity[i] = devp->config->accsensitivity[i];
774 }
775 }
776 else if(devp->config->accfullscale == LSM303AGR_ACC_FS_8G) {
777 devp->accfullscale = LSM303AGR_ACC_8G;
778 for(i = 0; i < LSM303AGR_ACC_NUMBER_OF_AXES; i++) {
779 if(devp->config->accsensitivity == NULL)
780 devp->accsensitivity[i] = LSM303AGR_ACC_SENS_8G;
781 else
782 devp->accsensitivity[i] = devp->config->accsensitivity[i];
783 }
784 }
785 else if(devp->config->accfullscale == LSM303AGR_ACC_FS_16G) {
786 devp->accfullscale = LSM303AGR_ACC_16G;
787 for(i = 0; i < LSM303AGR_ACC_NUMBER_OF_AXES; i++) {
788 if(devp->config->accsensitivity == NULL)
789 devp->accsensitivity[i] = LSM303AGR_ACC_SENS_16G;
790 else
791 devp->accsensitivity[i] = devp->config->accsensitivity[i];
792 }
793 }
794 else
795 osalDbgAssert(FALSE, "lsm303dlhcStart(), accelerometer full scale issue");
796
797 /* Storing bias information */
798 if(devp->config->accbias != NULL)
799 for(i = 0; i < LSM303AGR_ACC_NUMBER_OF_AXES; i++)
800 devp->accbias[i] = devp->config->accbias[i];
801 else
802 for(i = 0; i < LSM303AGR_ACC_NUMBER_OF_AXES; i++)
803 devp->accbias[i] = LSM303AGR_ACC_BIAS;
804
805#if LSM303AGR_SHARED_I2C
806 i2cAcquireBus((devp)->config->i2cp);
807#endif /* LSM303AGR_SHARED_I2C */
808 i2cStart((devp)->config->i2cp, (devp)->config->i2ccfg);
809
810 lsm303agrI2CWriteRegister(devp->config->i2cp, LSM303AGR_SAD_ACC, cr, 4);
811
812#if LSM303AGR_SHARED_I2C
813 i2cReleaseBus((devp)->config->i2cp);
814#endif /* LSM303AGR_SHARED_I2C */
815
816 /* Configuring Compass subsystem */
817 /* Multiple write starting address.*/
818 cr[0] = LSM303AGR_AD_CFG_REG_A_M;
819
820 /* Control register A configuration block.*/
821 {
822 cr[1] = devp->config->compoutputdatarate;
823#if LSM303AGR_USE_ADVANCED || defined(__DOXYGEN__)
824 cr[1] |= devp->config->compmode | devp->config->complp;
825#endif
826 }
827
828 /* Control register B configuration block.*/
829 {
830 cr[2] = 0;
831 }
832
833 /* Control register C configuration block.*/
834 {
835 cr[3] = 0;
836 }
837
838#if LSM303AGR_SHARED_I2C
839 i2cAcquireBus((devp)->config->i2cp);
840 i2cStart((devp)->config->i2cp, (devp)->config->i2ccfg);
841#endif /* LSM303AGR_SHARED_I2C */
842
843 lsm303agrI2CWriteRegister(devp->config->i2cp, LSM303AGR_SAD_COMP,
844 cr, 3);
845
846#if LSM303AGR_SHARED_I2C
847 i2cReleaseBus((devp)->config->i2cp);
848#endif /* LSM303AGR_SHARED_I2C */
849
850 devp->compfullscale = LSM303AGR_COMP_50GA;
851 for(i = 0; i < LSM303AGR_COMP_NUMBER_OF_AXES; i++) {
852 if(devp->config->compsensitivity == NULL) {
853 devp->compsensitivity[i] = LSM303AGR_COMP_SENS_50GA;
854 }
855 else {
856 devp->compsensitivity[i] = devp->config->compsensitivity[i];
857 }
858 }
859
860 /* This is the MEMS transient recovery time */
861 osalThreadSleepMilliseconds(5);
862
863 devp->state = LSM303AGR_READY;
864}
865
866/**
867 * @brief Deactivates the LSM303AGR Complex Driver peripheral.
868 *
869 * @param[in] devp pointer to the @p LSM303AGRDriver object
870 *
871 * @api
872 */
873void lsm303agrStop(LSM303AGRDriver *devp) {
874 uint8_t cr[2];
875 osalDbgCheck(devp != NULL);
876
877 osalDbgAssert((devp->state == LSM303AGR_STOP) ||
878 (devp->state == LSM303AGR_READY),
879 "lsm303agrStop(), invalid state");
880
881 if (devp->state == LSM303AGR_READY) {
882#if LSM303AGR_SHARED_I2C
883 i2cAcquireBus((devp)->config->i2cp);
884 i2cStart((devp)->config->i2cp, (devp)->config->i2ccfg);
885#endif /* LSM303AGR_SHARED_I2C */
886
887 /* Disabling accelerometer. */
888 cr[0] = LSM303AGR_AD_CTRL_REG1_A;
889 cr[1] = LSM303AGR_ACC_AE_DISABLED | LSM303AGR_ACC_ODR_PD;
890 lsm303agrI2CWriteRegister(devp->config->i2cp, LSM303AGR_SAD_ACC,
891 cr, 1);
892
893 /* Disabling compass. */
894 cr[0] = LSM303AGR_AD_CFG_REG_A_M;
895 cr[1] = LSM303AGR_COMP_MODE_IDLE;
896 lsm303agrI2CWriteRegister(devp->config->i2cp, LSM303AGR_SAD_COMP,
897 cr, 1);
898
899 i2cStop((devp)->config->i2cp);
900#if LSM303AGR_SHARED_I2C
901 i2cReleaseBus((devp)->config->i2cp);
902#endif /* LSM303AGR_SHARED_I2C */
903 }
904 devp->state = LSM303AGR_STOP;
905}
906/** @} */
generated by cgit v1.2.3 (git 2.25.1) at 2025-08-30 21:52:09 +0100