49 #if (LSM6DSL_USE_I2C) || defined(__DOXYGEN__)
64 uint8_t* rxbuf,
size_t n) {
66 return i2cMasterTransmitTimeout(i2cp, sad, ®, 1, rxbuf, n,
83 #define lsm6dslI2CWriteRegister(i2cp, sad, txbuf, n) \
84 i2cMasterTransmitTimeout(i2cp, sad, txbuf, n + 1, NULL, 0, \
123 osalDbgCheck((ip != NULL) && (axes != NULL));
129 "acc_read_raw(), invalid state");
131 osalDbgAssert((devp->config->i2cp->state == I2C_READY),
132 "acc_read_raw(), channel not ready");
134 #if LSM6DSL_SHARED_I2C
135 i2cAcquireBus(devp->config->i2cp);
136 i2cStart(devp->config->i2cp,
137 devp->config->i2ccfg);
141 LSM6DSL_AD_OUTX_L_XL, buff,
144 #if LSM6DSL_SHARED_I2C
145 i2cReleaseBus(devp->config->i2cp);
150 tmp = buff[2 * i] + (buff[2 * i + 1] << 8);
151 axes[i] = (int32_t)tmp;
179 osalDbgCheck((ip != NULL) && (axes != NULL));
185 "acc_read_cooked(), invalid state");
189 axes[i] = (raw[i] * devp->accsensitivity[i]) - devp->accbias[i];
211 osalDbgCheck((ip != NULL) && (bp != NULL));
217 "acc_set_bias(), invalid state");
220 devp->accbias[i] = bp[i];
240 osalDbgCheck(ip != NULL);
246 "acc_reset_bias(), invalid state");
249 devp->accbias[i] = LSM6DSL_ACC_BIAS;
273 osalDbgCheck((ip != NULL) && (sp != NULL));
276 "acc_set_sensivity(), invalid state");
279 devp->accsensitivity[i] = sp[i];
299 osalDbgCheck(ip != NULL);
305 "acc_reset_sensivity(), invalid state");
309 devp->accsensitivity[i] = LSM6DSL_ACC_SENS_2G;
312 devp->accsensitivity[i] = LSM6DSL_ACC_SENS_4G;
315 devp->accsensitivity[i] = LSM6DSL_ACC_SENS_8G;
318 devp->accsensitivity[i] = LSM6DSL_ACC_SENS_16G;
320 osalDbgAssert(FALSE,
"reset_sensivity(), accelerometer full scale issue");
344 osalDbgCheck(devp != NULL);
347 "acc_set_full_scale(), invalid state");
348 osalDbgAssert((devp->config->i2cp->state == I2C_READY),
349 "acc_set_full_scale(), channel not ready");
353 newfs = LSM6DSL_ACC_2G;
356 newfs = LSM6DSL_ACC_4G;
359 newfs = LSM6DSL_ACC_8G;
362 newfs = LSM6DSL_ACC_16G;
369 if(newfs != devp->accfullscale) {
371 scale = newfs / devp->accfullscale;
372 devp->accfullscale = newfs;
374 #if LSM6DSL_SHARED_I2C
375 i2cAcquireBus(devp->config->i2cp);
376 i2cStart(devp->config->i2cp,
377 devp->config->i2ccfg);
382 devp->config->slaveaddress,
383 LSM6DSL_AD_CTRL1_XL, &buff[1], 1);
385 #if LSM6DSL_SHARED_I2C
386 i2cReleaseBus(devp->config->i2cp);
392 buff[1] &= ~(LSMDSL_CTRL1_XL_FS_MASK);
394 buff[0] = LSM6DSL_AD_CTRL1_XL;
396 #if LSM6DSL_SHARED_I2C
397 i2cAcquireBus(devp->config->i2cp);
398 i2cStart(devp->config->i2cp, devp->config->i2ccfg);
402 devp->config->slaveaddress, buff, 1);
404 #if LSM6DSL_SHARED_I2C
405 i2cReleaseBus(devp->config->i2cp);
413 devp->accsensitivity[i] *= scale;
414 devp->accbias[i] *= scale;
455 osalDbgCheck((ip != NULL) && (axes != NULL));
461 "gyro_read_raw(), invalid state");
463 osalDbgAssert((devp->config->i2cp->state == I2C_READY),
464 "gyro_read_raw(), channel not ready");
466 #if LSM6DSL_SHARED_I2C
467 i2cAcquireBus(devp->config->i2cp);
468 i2cStart(devp->config->i2cp,
469 devp->config->i2ccfg);
473 LSM6DSL_AD_OUTX_L_G, buff,
476 #if LSM6DSL_SHARED_I2C
477 i2cReleaseBus(devp->config->i2cp);
482 tmp = buff[2 * i] + (buff[2 * i + 1] << 8);
483 axes[i] = (int32_t)tmp;
511 osalDbgCheck((ip != NULL) && (axes != NULL));
517 "gyro_read_cooked(), invalid state");
521 axes[i] = (raw[i] * devp->gyrosensitivity[i]) - devp->gyrobias[i];
545 osalDbgCheck(ip != NULL);
551 "gyro_sample_bias(), invalid state");
553 osalDbgAssert((devp->config->i2cp->state == I2C_READY),
554 "gyro_sample_bias(), channel not ready");
569 devp->gyrobias[i] *= devp->gyrosensitivity[i];
591 osalDbgCheck((ip != NULL) && (bp != NULL));
597 "gyro_set_bias(), invalid state");
600 devp->gyrobias[i] = bp[i];
620 osalDbgCheck(ip != NULL);
626 "gyro_reset_bias(), invalid state");
629 devp->gyrobias[i] = LSM6DSL_GYRO_BIAS;
650 osalDbgCheck((ip != NULL) && (sp !=NULL));
656 "gyro_set_sensivity(), invalid state");
659 devp->gyrosensitivity[i] = sp[i];
679 osalDbgCheck(ip != NULL);
685 "gyro_reset_sensivity(), invalid state");
688 devp->gyrosensitivity[i] = LSM6DSL_GYRO_SENS_125DPS;
691 devp->gyrosensitivity[i] = LSM6DSL_GYRO_SENS_250DPS;
694 devp->gyrosensitivity[i] = LSM6DSL_GYRO_SENS_500DPS;
697 devp->gyrosensitivity[i] = LSM6DSL_GYRO_SENS_1000DPS;
700 devp->gyrosensitivity[i] = LSM6DSL_GYRO_SENS_2000DPS;
702 osalDbgAssert(FALSE,
"gyro_reset_sensivity(), full scale issue");
726 osalDbgCheck(devp != NULL);
729 "gyro_set_full_scale(), invalid state");
731 osalDbgAssert((devp->config->i2cp->state == I2C_READY),
732 "gyro_set_full_scale(), channel not ready");
736 newfs = LSM6DSL_GYRO_125DPS;
739 newfs = LSM6DSL_GYRO_250DPS;
742 newfs = LSM6DSL_GYRO_500DPS;
745 newfs = LSM6DSL_GYRO_1000DPS;
748 newfs = LSM6DSL_GYRO_2000DPS;
754 if(newfs != devp->gyrofullscale) {
755 scale = newfs / devp->gyrofullscale;
756 devp->gyrofullscale = newfs;
759 #if LSM6DSL_SHARED_I2C
760 i2cAcquireBus(devp->config->i2cp);
761 i2cStart(devp->config->i2cp,
762 devp->config->i2ccfg);
767 devp->config->slaveaddress,
768 LSM6DSL_AD_CTRL2_G, &buff[1], 1);
770 #if LSM6DSL_SHARED_I2C
771 i2cReleaseBus(devp->config->i2cp);
775 buff[1] &= ~(LSMDSL_CTRL2_G_FS_MASK);
777 buff[0] = LSM6DSL_AD_CTRL2_G;
780 #if LSM6DSL_SHARED_I2C
781 i2cAcquireBus(devp->config->i2cp);
782 i2cStart(devp->config->i2cp,
783 devp->config->i2ccfg);
789 #if LSM6DSL_SHARED_I2C
790 i2cReleaseBus(devp->config->i2cp);
796 devp->gyrosensitivity[i] *= scale;
797 devp->gyrobias[i] *= scale;
833 devp->
vmt = &vmt_device;
856 osalDbgCheck((devp != NULL) && (config != NULL));
860 "lsm6dslStart(), invalid state");
862 devp->config = config;
866 cr[0] = LSM6DSL_AD_CTRL3_C;
867 cr[1] = LSMDSL_CTRL3_C_IF_INC;
870 #if LSM6DSL_SHARED_I2C
871 i2cAcquireBus(devp->config->i2cp);
874 i2cStart(devp->config->i2cp, devp->config->i2ccfg);
878 #if LSM6DSL_SHARED_I2C
879 i2cReleaseBus(devp->config->i2cp);
885 cr[0] = LSM6DSL_AD_CTRL1_XL;
888 cr[1] = devp->config->accoutdatarate |
889 devp->config->accfullscale;
893 cr[2] = devp->config->gyrooutdatarate |
894 devp->config->gyrofullscale;
898 cr[3] = LSMDSL_CTRL3_C_IF_INC;
899 #if LSM6DSL_USE_ADVANCED || defined(__DOXYGEN__)
900 cr[3] |= devp->config->endianness | devp->config->blockdataupdate;
906 #if LSM6DSL_USE_ADVANCED || defined(__DOXYGEN__)
908 cr[4] |= LSMDSL_CTRL4_C_LPF1_SEL_G;
922 #if LSM6DSL_USE_ADVANCED || defined(__DOXYGEN__)
923 cr[6] |= devp->config->acclpmode;
926 #if LSM6DSL_USE_ADVANCED || defined(__DOXYGEN__)
928 cr[6] |= devp->config->gyrolowpassfilter;
938 #if LSM6DSL_USE_ADVANCED || defined(__DOXYGEN__)
939 cr[7] |= devp->config->gyrolpmode;
957 #if LSM6DSL_SHARED_I2C
958 i2cAcquireBus(devp->config->i2cp);
959 i2cStart(devp->config->i2cp, devp->config->i2ccfg);
965 #if LSM6DSL_SHARED_I2C
966 i2cReleaseBus(devp->config->i2cp);
973 if(devp->config->accsensitivity == NULL)
974 devp->accsensitivity[i] = LSM6DSL_ACC_SENS_2G;
976 devp->accsensitivity[i] = devp->config->accsensitivity[i];
978 devp->accfullscale = LSM6DSL_ACC_2G;
982 if(devp->config->accsensitivity == NULL)
983 devp->accsensitivity[i] = LSM6DSL_ACC_SENS_4G;
985 devp->accsensitivity[i] = devp->config->accsensitivity[i];
987 devp->accfullscale = LSM6DSL_ACC_4G;
991 if(devp->config->accsensitivity == NULL)
992 devp->accsensitivity[i] = LSM6DSL_ACC_SENS_8G;
994 devp->accsensitivity[i] = devp->config->accsensitivity[i];
996 devp->accfullscale = LSM6DSL_ACC_8G;
1000 if(devp->config->accsensitivity == NULL)
1001 devp->accsensitivity[i] = LSM6DSL_ACC_SENS_16G;
1003 devp->accsensitivity[i] = devp->config->accsensitivity[i];
1005 devp->accfullscale = LSM6DSL_ACC_16G;
1008 osalDbgAssert(FALSE,
"lsm6dslStart(), accelerometer full scale issue");
1011 if(devp->config->accbias != NULL)
1013 devp->accbias[i] = devp->config->accbias[i];
1016 devp->accbias[i] = LSM6DSL_ACC_BIAS;
1020 if(devp->config->gyrosensitivity == NULL)
1021 devp->gyrosensitivity[i] = LSM6DSL_GYRO_SENS_125DPS;
1023 devp->gyrosensitivity[i] = devp->config->gyrosensitivity[i];
1025 devp->gyrofullscale = LSM6DSL_GYRO_125DPS;
1029 if(devp->config->gyrosensitivity == NULL)
1030 devp->gyrosensitivity[i] = LSM6DSL_GYRO_SENS_250DPS;
1032 devp->gyrosensitivity[i] = devp->config->gyrosensitivity[i];
1034 devp->gyrofullscale = LSM6DSL_GYRO_250DPS;
1038 if(devp->config->gyrosensitivity == NULL)
1039 devp->gyrosensitivity[i] = LSM6DSL_GYRO_SENS_500DPS;
1041 devp->gyrosensitivity[i] = devp->config->gyrosensitivity[i];
1043 devp->gyrofullscale = LSM6DSL_GYRO_500DPS;
1047 if(devp->config->gyrosensitivity == NULL)
1048 devp->gyrosensitivity[i] = LSM6DSL_GYRO_SENS_1000DPS;
1050 devp->gyrosensitivity[i] = devp->config->gyrosensitivity[i];
1052 devp->gyrofullscale = LSM6DSL_GYRO_1000DPS;
1056 if(devp->config->gyrosensitivity == NULL)
1057 devp->gyrosensitivity[i] = LSM6DSL_GYRO_SENS_2000DPS;
1059 devp->gyrosensitivity[i] = devp->config->gyrosensitivity[i];
1061 devp->gyrofullscale = LSM6DSL_GYRO_2000DPS;
1064 osalDbgAssert(FALSE,
"lsm6dslStart(), gyroscope full scale issue");
1067 if(devp->config->gyrobias != NULL)
1069 devp->gyrobias[i] = devp->config->gyrobias[i];
1072 devp->gyrobias[i] = LSM6DSL_GYRO_BIAS;
1075 osalThreadSleepMilliseconds(5);
1090 osalDbgCheck(devp != NULL);
1093 "lsm6dslStop(), invalid state");
1097 #if LSM6DSL_SHARED_I2C
1098 i2cAcquireBus(devp->config->i2cp);
1099 i2cStart(devp->config->i2cp, devp->config->i2ccfg);
1103 cr[0] = LSM6DSL_AD_CTRL1_XL;
1111 i2cStop(devp->config->i2cp);
1112 #if LSM6DSL_SHARED_I2C
1113 i2cReleaseBus(devp->config->i2cp);