From 1a503b3dfaaeed2e430f8e530d4c3a5fd3a015bc Mon Sep 17 00:00:00 2001 From: Rocco Marco Guglielmi Date: Tue, 15 Mar 2016 20:24:03 +0000 Subject: [PATCH] Fixed l3gd20 and lsm6ds0 makefiles. Resumed development on lsm6ds0 gyro subsystem git-svn-id: svn://svn.code.sf.net/p/chibios/svn/trunk@9120 35acf78f-673a-0410-8e92-d51de3d6d3f4 --- os/ex/ST/l3gd20.mk | 2 +- os/ex/ST/lsm6ds0.c | 126 ++++++++++++++++++++++++++++++++++++++++++-- os/ex/ST/lsm6ds0.h | 21 +++++++- os/ex/ST/lsm6ds0.mk | 2 +- 4 files changed, 143 insertions(+), 8 deletions(-) diff --git a/os/ex/ST/l3gd20.mk b/os/ex/ST/l3gd20.mk index cd2b79684..eb547a739 100644 --- a/os/ex/ST/l3gd20.mk +++ b/os/ex/ST/l3gd20.mk @@ -2,5 +2,5 @@ L3GD20SRC := $(CHIBIOS)/os/ex/ST/l3gd20.c # Required include directories -L3GD20INC := $(CHIBIOS)/os/hal/lib/peripherals/sensors/include \ +L3GD20INC := $(CHIBIOS)/os/hal/lib/peripherals/sensors \ $(CHIBIOS)/os/ex/ST \ No newline at end of file diff --git a/os/ex/ST/lsm6ds0.c b/os/ex/ST/lsm6ds0.c index 4aa7a7eb0..72e79b558 100644 --- a/os/ex/ST/lsm6ds0.c +++ b/os/ex/ST/lsm6ds0.c @@ -265,14 +265,15 @@ msg_t lsm6ds0I2CWriteRegister(I2CDriver *i2cp, lsm6ds0_sad_t sad, uint8_t reg, */ static size_t acc_get_axes_number(void *ip) { - osalDbgCheck(ip != NULL); + osalDbgCheck((ip != NULL) && + (((LSM6DS0Driver *)ip)->config->acccfg != NULL)); return LSM6DS0_ACC_NUMBER_OF_AXES; } static msg_t acc_read_raw(void *ip, int32_t axes[LSM6DS0_ACC_NUMBER_OF_AXES]) { - osalDbgCheck((ip != NULL) && (axes != NULL)); - + osalDbgCheck(((ip != NULL) && (axes != NULL)) && + (((LSM6DS0Driver *)ip)->config->acccfg != NULL)); osalDbgAssert((((LSM6DS0Driver *)ip)->state == LSM6DS0_READY), "acc_read_raw(), invalid state"); @@ -320,10 +321,11 @@ static msg_t acc_read_cooked(void *ip, float axes[]) { int32_t raw[LSM6DS0_ACC_NUMBER_OF_AXES]; msg_t msg; - osalDbgCheck((ip != NULL) && (axes != NULL)); + osalDbgCheck(((ip != NULL) && (axes != NULL)) && + (((LSM6DS0Driver *)ip)->config->acccfg != NULL)); osalDbgAssert((((LSM6DS0Driver *)ip)->state == LSM6DS0_READY), - "read_cooked(), invalid state"); + "acc_read_cooked(), invalid state"); msg = acc_read_raw(ip, raw); for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES ; i++){ @@ -338,10 +340,123 @@ static msg_t acc_read_cooked(void *ip, float axes[]) { return msg; } +static size_t gyro_get_axes_number(void *ip) { + + osalDbgCheck((ip != NULL) && + (((LSM6DS0Driver *)ip)->config->gyrocfg != NULL)); + return LSM6DS0_GYRO_NUMBER_OF_AXES; +} + +static msg_t gyro_read_raw(void *ip, int32_t axes[LSM6DS0_GYRO_NUMBER_OF_AXES]) { + + osalDbgCheck(((ip != NULL) && (axes != NULL)) && + (((LSM6DS0Driver *)ip)->config->gyrocfg != NULL)); + osalDbgAssert((((LSM6DS0Driver *)ip)->state == LSM6DS0_READY), + "gyro_read_raw(), invalid state"); + +#if LSM6DS0_USE_I2C + osalDbgAssert((((LSM6DS0Driver *)ip)->config->i2cp->state == I2C_READY), + "gyro_read_raw(), channel not ready"); +#if LSM6DS0_SHARED_I2C + i2cAcquireBus(((LSM6DS0Driver *)ip)->config->i2cp); + i2cStart(((LSM6DS0Driver *)ip)->config->i2cp, + ((LSM6DS0Driver *)ip)->config->i2ccfg); +#endif /* LSM6DS0_SHARED_I2C */ + if(((LSM6DS0Driver *)ip)->config->gyrocfg->axesenabling & LSM6DS0_GYRO_AE_X){ + axes[0] = (int16_t)(lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp, + ((LSM6DS0Driver *)ip)->config->slaveaddress, + LSM6DS0_AD_OUT_X_L_G, NULL)); + axes[0] += (int16_t)(lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp, + ((LSM6DS0Driver *)ip)->config->slaveaddress, + LSM6DS0_AD_OUT_X_H_G, NULL) << 8); + } + if(((LSM6DS0Driver *)ip)->config->gyrocfg->axesenabling & LSM6DS0_GYRO_AE_Y){ + axes[1] = (int16_t)(lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp, + ((LSM6DS0Driver *)ip)->config->slaveaddress, + LSM6DS0_AD_OUT_Y_L_G, NULL)); + axes[1] += (int16_t)(lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp, + ((LSM6DS0Driver *)ip)->config->slaveaddress, + LSM6DS0_AD_OUT_Y_H_G, NULL) << 8); + } + if(((LSM6DS0Driver *)ip)->config->gyrocfg->axesenabling & LSM6DS0_GYRO_AE_Z){ + axes[2] = (int16_t)(lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp, + ((LSM6DS0Driver *)ip)->config->slaveaddress, + LSM6DS0_AD_OUT_Z_L_G, NULL)); + axes[2] += (int16_t)(lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp, + ((LSM6DS0Driver *)ip)->config->slaveaddress, + LSM6DS0_AD_OUT_Z_H_G, NULL) << 8); + } +#if LSM6DS0_SHARED_I2C + i2cReleaseBus(((LSM6DS0Driver *)ip)->config->i2cp); +#endif /* LSM6DS0_SHARED_I2C */ +#endif + return MSG_OK; +} + +static msg_t gyro_read_cooked(void *ip, float axes[]) { + uint32_t i; + int32_t raw[LSM6DS0_GYRO_NUMBER_OF_AXES]; + msg_t msg; + + osalDbgCheck(((ip != NULL) && (axes != NULL)) && + (((LSM6DS0Driver *)ip)->config->gyrocfg != NULL)); + + osalDbgAssert((((LSM6DS0Driver *)ip)->state == LSM6DS0_READY), + "gyro_read_cooked(), invalid state"); + + msg = acc_read_raw(ip, raw); + for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES ; i++){ + axes[i] = raw[i] * ((LSM6DS0Driver *)ip)->gyrosens; + } + return msg; +} + +static msg_t gyro_reset_calibration(void *ip) { + uint32_t i; + + osalDbgCheck(ip != NULL); + + osalDbgAssert((((LSM6DS0Driver *)ip)->state == LSM6DS0_READY) || + (((LSM6DS0Driver *)ip)->state == LSM6DS0_STOP), + "reset_calibration(), invalid state"); + + for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++) + ((LSM6DS0Driver *)ip)->gyrobias[i] = 0; + return MSG_OK; +} + +static msg_t gyro_calibrate(void *ip) { + uint32_t i, j; + int32_t raw[LSM6DS0_GYRO_NUMBER_OF_AXES]; + int32_t buff[LSM6DS0_GYRO_NUMBER_OF_AXES] = {0, 0, 0}; + + osalDbgCheck(ip != NULL); + + osalDbgAssert((((LSM6DS0Driver *)ip)->state == LSM6DS0_READY), + "gyro_calibrate(), invalid state"); + + for(i = 0; i < LSM6DS0_GYRO_BIAS_ACQ_TIMES; i++){ + gyro_read_raw(ip, raw); + for(j = 0; j < LSM6DS0_GYRO_NUMBER_OF_AXES; j++){ + buff[j] += raw[j]; + } + osalThreadSleepMicroseconds(LSM6DS0_GYRO_BIAS_SETTLING_uS); + } + + for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++){ + ((LSM6DS0Driver *)ip)->gyrobias[i] = buff[i] / LSM6DS0_GYRO_BIAS_ACQ_TIMES; + } + return MSG_OK; +} + static const struct LSM6DS0ACCVMT vmt_baseaccelerometer = { acc_get_axes_number, acc_read_raw, acc_read_cooked }; +static const struct LSM6DS0GYROVMT vmt_basegyroscope = { + gyro_get_axes_number, gyro_read_raw, gyro_read_cooked, + gyro_reset_calibration, gyro_calibrate +}; /*===========================================================================*/ /* Driver exported functions. */ /*===========================================================================*/ @@ -356,6 +471,7 @@ static const struct LSM6DS0ACCVMT vmt_baseaccelerometer = { void lsm6ds0ObjectInit(LSM6DS0Driver *devp) { devp->vmt_baseaccelerometer = &vmt_baseaccelerometer; + devp->vmt_basegyroscope = &vmt_basegyroscope; devp->state = LSM6DS0_STOP; devp->config = NULL; } diff --git a/os/ex/ST/lsm6ds0.h b/os/ex/ST/lsm6ds0.h index 6adf58a94..06f41419c 100644 --- a/os/ex/ST/lsm6ds0.h +++ b/os/ex/ST/lsm6ds0.h @@ -74,6 +74,23 @@ #if !defined(LSM6DS0_SHARED_I2C) || defined(__DOXYGEN__) #define LSM6DS0_SHARED_I2C FALSE #endif + +/** + * @brief Number of acquisitions for gyroscope bias removal + * @details This is the number of acquisitions performed to compute the + * bias. A repetition is required in order to remove noise. + */ +#if !defined(LSM6DS0_GYRO_BIAS_ACQ_TIMES) || defined(__DOXYGEN__) +#define LSM6DS0_GYRO_BIAS_ACQ_TIMES 50 +#endif + +/** + * @brief Settling time for gyroscope bias removal + * @details This is the time between each bias acquisition. + */ +#if !defined(LSM6DS0_GYRO_BIAS_SETTLING_uS) || defined(__DOXYGEN__) +#define LSM6DS0_GYRO_BIAS_SETTLING_uS 5000 +#endif /** @} */ /*===========================================================================*/ @@ -518,7 +535,9 @@ struct LSM6DS0GYROVMT { /* Current accelerometer sensitivity.*/ \ float accsens; \ /* Current gyroscope sensitivity.*/ \ - float gyrosens; + float gyrosens; \ + /* Bias data.*/ \ + int32_t gyrobias[LSM6DS0_GYRO_NUMBER_OF_AXES]; /** * @brief LSM6DS0 6-axis accelerometer/gyroscope class. diff --git a/os/ex/ST/lsm6ds0.mk b/os/ex/ST/lsm6ds0.mk index 313301f0f..3acd24845 100644 --- a/os/ex/ST/lsm6ds0.mk +++ b/os/ex/ST/lsm6ds0.mk @@ -2,5 +2,5 @@ LSM6DS0SRC := $(CHIBIOS)/os/ex/ST/lsm6ds0.c # Required include directories -LSM6DS0INC := $(CHIBIOS)/os/hal/lib/peripherals/sensors/include \ +LSM6DS0INC := $(CHIBIOS)/os/hal/lib/peripherals/sensors \ $(CHIBIOS)/os/ex/ST \ No newline at end of file