From df0a27871a256b6fa1b7a8e18cfd74f2c86b9119 Mon Sep 17 00:00:00 2001 From: Rocco Marco Guglielmi Date: Wed, 14 Mar 2018 11:07:17 +0000 Subject: Completed base support for LSM6DSL added base demo. git-svn-id: svn://svn.code.sf.net/p/chibios/svn/trunk@11740 110e8d01-0319-4d1e-a829-52ad28d1bb01 --- os/ex/ST/lsm6dsl.c | 227 +++++++++++++++++++++++++++------------------------- os/ex/ST/lsm6dsl.h | 26 +++--- os/ex/ST/lsm6dsl.mk | 2 +- 3 files changed, 131 insertions(+), 124 deletions(-) (limited to 'os') diff --git a/os/ex/ST/lsm6dsl.c b/os/ex/ST/lsm6dsl.c index 6a1fc34fc..24b78c87f 100644 --- a/os/ex/ST/lsm6dsl.c +++ b/os/ex/ST/lsm6dsl.c @@ -138,7 +138,7 @@ static msg_t acc_read_raw(void *ip, int32_t axes[]) { #endif /* LSM6DSL_SHARED_I2C */ msg = lsm6dslI2CReadRegister(devp->config->i2cp, devp->config->slaveaddress, - LSM6DSL_AD_OUT_X_L_XL, buff, + LSM6DSL_AD_OUTX_L_XL, buff, LSM6DSL_ACC_NUMBER_OF_AXES * 2); #if LSM6DSL_SHARED_I2C @@ -381,7 +381,7 @@ static msg_t acc_set_full_scale(LSM6DSLDriver *devp, /* Updating register.*/ msg = lsm6dslI2CReadRegister(devp->config->i2cp, devp->config->slaveaddress, - LSM6DSL_AD_CTRL_REG6_XL, &buff[1], 1); + LSM6DSL_AD_CTRL1_XL, &buff[1], 1); #if LSM6DSL_SHARED_I2C i2cReleaseBus(devp->config->i2cp); @@ -390,9 +390,9 @@ static msg_t acc_set_full_scale(LSM6DSLDriver *devp, if(msg != MSG_OK) return msg; - buff[1] &= ~(LSM6DSL_CTRL_REG6_XL_FS_MASK); + buff[1] &= ~(LSMDSL_CTRL1_XL_FS_MASK); buff[1] |= fs; - buff[0] = LSM6DSL_AD_CTRL_REG6_XL; + buff[0] = LSM6DSL_AD_CTRL1_XL; #if LSM6DSL_SHARED_I2C i2cAcquireBus(devp->config->i2cp); @@ -471,7 +471,7 @@ static msg_t gyro_read_raw(void *ip, int32_t axes[LSM6DSL_GYRO_NUMBER_OF_AXES]) #endif /* LSM6DSL_SHARED_I2C */ msg = lsm6dslI2CReadRegister(devp->config->i2cp, devp->config->slaveaddress, - LSM6DSL_AD_OUT_X_L_G, buff, + LSM6DSL_AD_OUTX_L_G, buff, LSM6DSL_GYRO_NUMBER_OF_AXES * 2); #if LSM6DSL_SHARED_I2C @@ -684,13 +684,18 @@ static msg_t gyro_reset_sensivity(void *ip) { osalDbgAssert((devp->state == LSM6DSL_READY), "gyro_reset_sensivity(), invalid state"); - - if(devp->config->gyrofullscale == LSM6DSL_GYRO_FS_245DPS) + if(devp->config->gyrofullscale == LSM6DSL_GYRO_FS_125DPS) + for(i = 0; i < LSM6DSL_GYRO_NUMBER_OF_AXES; i++) + devp->gyrosensitivity[i] = LSM6DSL_GYRO_SENS_125DPS; + else if(devp->config->gyrofullscale == LSM6DSL_GYRO_FS_250DPS) for(i = 0; i < LSM6DSL_GYRO_NUMBER_OF_AXES; i++) - devp->gyrosensitivity[i] = LSM6DSL_GYRO_SENS_245DPS; + devp->gyrosensitivity[i] = LSM6DSL_GYRO_SENS_250DPS; else if(devp->config->gyrofullscale == LSM6DSL_GYRO_FS_500DPS) for(i = 0; i < LSM6DSL_GYRO_NUMBER_OF_AXES; i++) devp->gyrosensitivity[i] = LSM6DSL_GYRO_SENS_500DPS; + else if(devp->config->gyrofullscale == LSM6DSL_GYRO_FS_1000DPS) + for(i = 0; i < LSM6DSL_GYRO_NUMBER_OF_AXES; i++) + devp->gyrosensitivity[i] = LSM6DSL_GYRO_SENS_1000DPS; else if(devp->config->gyrofullscale == LSM6DSL_GYRO_FS_2000DPS) for(i = 0; i < LSM6DSL_GYRO_NUMBER_OF_AXES; i++) devp->gyrosensitivity[i] = LSM6DSL_GYRO_SENS_2000DPS; @@ -728,12 +733,18 @@ static msg_t gyro_set_full_scale(LSM6DSLDriver *devp, lsm6dsl_gyro_fs_t fs) { "gyro_set_full_scale(), channel not ready"); #endif - if(fs == LSM6DSL_GYRO_FS_245DPS) { - newfs = LSM6DSL_GYRO_245DPS; + if(fs == LSM6DSL_GYRO_FS_125DPS) { + newfs = LSM6DSL_GYRO_125DPS; + } + else if(fs == LSM6DSL_GYRO_FS_250DPS) { + newfs = LSM6DSL_GYRO_250DPS; } else if(fs == LSM6DSL_GYRO_FS_500DPS) { newfs = LSM6DSL_GYRO_500DPS; } + else if(fs == LSM6DSL_GYRO_FS_1000DPS) { + newfs = LSM6DSL_GYRO_1000DPS; + } else if(fs == LSM6DSL_GYRO_FS_2000DPS) { newfs = LSM6DSL_GYRO_2000DPS; } @@ -755,16 +766,16 @@ static msg_t gyro_set_full_scale(LSM6DSLDriver *devp, lsm6dsl_gyro_fs_t fs) { /* Updating register.*/ msg = lsm6dslI2CReadRegister(devp->config->i2cp, devp->config->slaveaddress, - LSM6DSL_AD_CTRL_REG1_G, &buff[1], 1); + LSM6DSL_AD_CTRL2_G, &buff[1], 1); #if LSM6DSL_SHARED_I2C i2cReleaseBus(devp->config->i2cp); #endif /* LSM6DSL_SHARED_I2C */ #endif /* LSM6DSL_USE_I2C */ - buff[1] &= ~(LSM6DSL_CTRL_REG1_G_FS_MASK); + buff[1] &= ~(LSMDSL_CTRL2_G_FS_MASK); buff[1] |= fs; - buff[0] = LSM6DSL_AD_CTRL_REG1_G; + buff[0] = LSM6DSL_AD_CTRL2_G; #if LSM6DSL_USE_I2C #if LSM6DSL_SHARED_I2C @@ -842,7 +853,7 @@ void lsm6dslObjectInit(LSM6DSLDriver *devp) { */ void lsm6dslStart(LSM6DSLDriver *devp, const LSM6DSLConfig *config) { uint32_t i; - uint8_t cr[5]; + uint8_t cr[11]; osalDbgCheck((devp != NULL) && (config != NULL)); osalDbgAssert((devp->state == LSM6DSL_STOP) || @@ -851,15 +862,10 @@ void lsm6dslStart(LSM6DSLDriver *devp, const LSM6DSLConfig *config) { devp->config = config; - /* Configuring common registers.*/ - - /* Control register 8 configuration block.*/ + /* Enforcing multiple write configuration.*/ { - cr[0] = LSM6DSL_AD_CTRL_REG8; - cr[1] = LSM6DSL_CTRL_REG8_IF_ADD_INC; -#if LSM6DSL_USE_ADVANCED || defined(__DOXYGEN__) - cr[1] |= devp->config->endianness | devp->config->blockdataupdate; -#endif + cr[0] = LSM6DSL_AD_CTRL3_C; + cr[1] = LSMDSL_CTRL3_C_IF_INC; } #if LSM6DSL_USE_I2C #if LSM6DSL_SHARED_I2C @@ -875,22 +881,77 @@ void lsm6dslStart(LSM6DSLDriver *devp, const LSM6DSLConfig *config) { #endif /* LSM6DSL_SHARED_I2C */ #endif /* LSM6DSL_USE_I2C */ - /* Configuring Accelerometer subsystem.*/ + /* Configuring all the control registers.*/ /* Multiple write starting address.*/ - cr[0] = LSM6DSL_AD_CTRL_REG5_XL; + cr[0] = LSM6DSL_AD_CTRL1_XL; + /* Control register 1 configuration block.*/ + { + cr[1] = devp->config->accoutdatarate | + devp->config->accfullscale; + } + /* Control register 2 configuration block.*/ + { + cr[2] = devp->config->gyrooutdatarate | + devp->config->gyrofullscale; + } + /* Control register 3 configuration block.*/ + { + cr[3] = LSMDSL_CTRL3_C_IF_INC; +#if LSM6DSL_USE_ADVANCED || defined(__DOXYGEN__) + cr[3] |= devp->config->endianness | devp->config->blockdataupdate; +#endif + } + /* Control register 4 configuration block.*/ + { + cr[4] = 0; +#if LSM6DSL_GYRO_USE_ADVANCED || defined(__DOXYGEN__) + if(devp->config->gyrolowpassfilter != LSM6DSL_GYRO_LPF_DISABLED) { + cr[4] |= LSMDSL_CTRL4_C_LPF1_SEL_G; + } + else { + /* Nothing to do. */ + } +#endif + } /* Control register 5 configuration block.*/ { - cr[1] = LSM6DSL_CTRL_REG5_XL_XEN_XL | LSM6DSL_CTRL_REG5_XL_YEN_XL | - LSM6DSL_CTRL_REG5_XL_ZEN_XL; + cr[5] = 0; + } + /* Control register 6 configuration block.*/ + { + cr[6] = 0; #if LSM6DSL_ACC_USE_ADVANCED || defined(__DOXYGEN__) - cr[1] |= devp->config->accdecmode; + cr[6] |= devp->config->acclpmode; + +#endif +#if LSM6DSL_GYRO_USE_ADVANCED || defined(__DOXYGEN__) + if(devp->config->gyrolowpassfilter != LSM6DSL_GYRO_LPF_DISABLED) { + cr[6] |= devp->config->gyrolowpassfilter; + } + else { + /* Nothing to do. */ + } #endif } + /* Control register 7 configuration block.*/ + { + cr[7] = 0; +#if LSM6DSL_GYRO_USE_ADVANCED || defined(__DOXYGEN__) + cr[7] |= devp->config->gyrolpmode; - /* Control register 6 configuration block.*/ +#endif + } + /* Control register 8 configuration block.*/ { - cr[2] = devp->config->accoutdatarate | - devp->config->accfullscale; + cr[8] = 0; + } + /* Control register 9 configuration block.*/ + { + cr[9] = 0; + } + /* Control register 10 configuration block.*/ + { + cr[10] = 0; } #if LSM6DSL_USE_I2C @@ -900,7 +961,7 @@ void lsm6dslStart(LSM6DSLDriver *devp, const LSM6DSLConfig *config) { #endif /* LSM6DSL_SHARED_I2C */ lsm6dslI2CWriteRegister(devp->config->i2cp, devp->config->slaveaddress, - cr, 2); + cr, 10); #if LSM6DSL_SHARED_I2C i2cReleaseBus(devp->config->i2cp); @@ -954,84 +1015,24 @@ void lsm6dslStart(LSM6DSLDriver *devp, const LSM6DSLConfig *config) { else for(i = 0; i < LSM6DSL_ACC_NUMBER_OF_AXES; i++) devp->accbias[i] = LSM6DSL_ACC_BIAS; - - /* Configuring Gyroscope subsystem.*/ - /* Multiple write starting address.*/ - cr[0] = LSM6DSL_AD_CTRL_REG1_G; - - /* Control register 1 configuration block.*/ - { - cr[1] = devp->config->gyrofullscale | - devp->config->gyrooutdatarate; -#if LSM6DSL_GYRO_USE_ADVANCED || defined(__DOXYGEN__) - cr[1] |= devp->config->gyrodecmode; -#endif - } - - /* Control register 2 configuration block.*/ - { - cr[2] = 0; -#if LSM6DSL_GYRO_USE_ADVANCED || defined(__DOXYGEN__) - cr[2] |= devp->config->gyrooutsel; -#endif - } - - /* Control register 3 configuration block.*/ - { - cr[3] = 0; -#if LSM6DSL_GYRO_USE_ADVANCED || defined(__DOXYGEN__) - cr[3] |= devp->config->gyrohpfenable | - devp->config->gyrolowmodecfg | - devp->config->gyrohpcfg; -#endif - } - /* Control register 4 configuration block.*/ - { - cr[4] = LSM6DSL_CTRL_REG4_XEN_G | LSM6DSL_CTRL_REG4_YEN_G | - LSM6DSL_CTRL_REG4_ZEN_G; - } -#if LSM6DSL_USE_I2C -#if LSM6DSL_SHARED_I2C - i2cAcquireBus(devp->config->i2cp); - i2cStart(devp->config->i2cp, devp->config->i2ccfg); -#endif /* LSM6DSL_SHARED_I2C */ - - lsm6dslI2CWriteRegister(devp->config->i2cp, devp->config->slaveaddress, - cr, 4); - -#if LSM6DSL_SHARED_I2C - i2cReleaseBus(devp->config->i2cp); -#endif /* LSM6DSL_SHARED_I2C */ -#endif /* LSM6DSL_USE_I2C */ - - cr[0] = LSM6DSL_AD_CTRL_REG9; - /* Control register 9 configuration block.*/ - { - cr[1] = 0; + if(devp->config->gyrofullscale == LSM6DSL_GYRO_FS_125DPS) { + for(i = 0; i < LSM6DSL_GYRO_NUMBER_OF_AXES; i++) { + if(devp->config->gyrosensitivity == NULL) + devp->gyrosensitivity[i] = LSM6DSL_GYRO_SENS_125DPS; + else + devp->gyrosensitivity[i] = devp->config->gyrosensitivity[i]; + } + devp->gyrofullscale = LSM6DSL_GYRO_125DPS; } -#if LSM6DSL_USE_I2C -#if LSM6DSL_SHARED_I2C - i2cAcquireBus(devp->config->i2cp); - i2cStart(devp->config->i2cp, devp->config->i2ccfg); -#endif /* LSM6DSL_SHARED_I2C */ - - lsm6dslI2CWriteRegister(devp->config->i2cp, devp->config->slaveaddress, - cr, 1); - -#if LSM6DSL_SHARED_I2C - i2cReleaseBus(devp->config->i2cp); -#endif /* LSM6DSL_SHARED_I2C */ -#endif /* LSM6DSL_USE_I2C */ - - if(devp->config->gyrofullscale == LSM6DSL_GYRO_FS_245DPS) { + else if(devp->config->gyrofullscale == LSM6DSL_GYRO_FS_250DPS) { for(i = 0; i < LSM6DSL_GYRO_NUMBER_OF_AXES; i++) { if(devp->config->gyrosensitivity == NULL) - devp->gyrosensitivity[i] = LSM6DSL_GYRO_SENS_245DPS; + devp->gyrosensitivity[i] = LSM6DSL_GYRO_SENS_250DPS; else devp->gyrosensitivity[i] = devp->config->gyrosensitivity[i]; } - devp->gyrofullscale = LSM6DSL_GYRO_245DPS; + devp->gyrofullscale = LSM6DSL_GYRO_250DPS; } else if(devp->config->gyrofullscale == LSM6DSL_GYRO_FS_500DPS) { for(i = 0; i < LSM6DSL_GYRO_NUMBER_OF_AXES; i++) { @@ -1042,6 +1043,15 @@ void lsm6dslStart(LSM6DSLDriver *devp, const LSM6DSLConfig *config) { } devp->gyrofullscale = LSM6DSL_GYRO_500DPS; } + else if(devp->config->gyrofullscale == LSM6DSL_GYRO_FS_1000DPS) { + for(i = 0; i < LSM6DSL_GYRO_NUMBER_OF_AXES; i++) { + if(devp->config->gyrosensitivity == NULL) + devp->gyrosensitivity[i] = LSM6DSL_GYRO_SENS_1000DPS; + else + devp->gyrosensitivity[i] = devp->config->gyrosensitivity[i]; + } + devp->gyrofullscale = LSM6DSL_GYRO_1000DPS; + } else if(devp->config->gyrofullscale == LSM6DSL_GYRO_FS_2000DPS) { for(i = 0; i < LSM6DSL_GYRO_NUMBER_OF_AXES; i++) { if(devp->config->gyrosensitivity == NULL) @@ -1090,17 +1100,14 @@ void lsm6dslStop(LSM6DSLDriver *devp) { i2cStart(devp->config->i2cp, devp->config->i2ccfg); #endif /* LSM6DSL_SHARED_I2C */ - /* Disabling accelerometer.*/ - cr[0] = LSM6DSL_AD_CTRL_REG6_XL; - cr[1] = 0; - lsm6dslI2CWriteRegister(devp->config->i2cp, devp->config->slaveaddress, - cr, 1); + cr[0] = LSM6DSL_AD_CTRL1_XL; + /* Disabling accelerometer.*/ + cr[1] = LSM6DSL_ACC_ODR_PD; /* Disabling gyroscope.*/ - cr[0] = LSM6DSL_AD_CTRL_REG9; - cr[1] = LSM6DSL_CTRL_REG9_SLEEP_G; + cr[2] = LSM6DSL_GYRO_ODR_PD; lsm6dslI2CWriteRegister(devp->config->i2cp, devp->config->slaveaddress, - cr, 1); + cr, 2); i2cStop(devp->config->i2cp); #if LSM6DSL_SHARED_I2C diff --git a/os/ex/ST/lsm6dsl.h b/os/ex/ST/lsm6dsl.h index 9816cee7d..7c3215a5b 100644 --- a/os/ex/ST/lsm6dsl.h +++ b/os/ex/ST/lsm6dsl.h @@ -243,10 +243,10 @@ #define LSMDSL_CTRL2_G_FS_125 (1 << 1) #define LSMDSL_CTRL2_G_FS_G0 (1 << 2) #define LSMDSL_CTRL2_G_FS_G1 (1 << 3) -#define LSMDSL_CTRL2_G_ODR_G (1 << 4) -#define LSMDSL_CTRL2_G_ODR_G (1 << 5) -#define LSMDSL_CTRL2_G_ODR_G (1 << 6) -#define LSMDSL_CTRL2_G_ODR_G (1 << 7) +#define LSMDSL_CTRL2_G_ODR_G0 (1 << 4) +#define LSMDSL_CTRL2_G_ODR_G1 (1 << 5) +#define LSMDSL_CTRL2_G_ODR_G2 (1 << 6) +#define LSMDSL_CTRL2_G_ODR_G3 (1 << 7) /** @} */ /** @@ -296,7 +296,7 @@ * @{ */ #define LSMDSL_CTRL6_C_FTYPE_0 (1 << 0) -#define LSMDSL_CTRL6_C_FTYPE_0 (1 << 1) +#define LSMDSL_CTRL6_C_FTYPE_1 (1 << 1) #define LSMDSL_CTRL6_C_USR_OFF_W (1 << 3) #define LSMDSL_CTRL6_C_XL_HM_MODE (1 << 4) #define LSMDSL_CTRL6_C_LVL2_EN (1 << 5) @@ -519,9 +519,9 @@ typedef enum { LSM6DSL_ACC_ODR_104Hz = 0x40, /**< ODR 104 Hz */ LSM6DSL_ACC_ODR_208Hz = 0x50, /**< ODR 208 Hz */ LSM6DSL_ACC_ODR_416Hz = 0x60, /**< ODR 416 Hz */ - LSM6DSL_ACC_ODR_833Hz = 0x70 /**< ODR 833 Hz */ - LSM6DSL_ACC_ODR_1P66Hz = 0x80 /**< ODR 1.66 kHz */ - LSM6DSL_ACC_ODR_3P33Hz = 0x90 /**< ODR 3.33 kHz */ + LSM6DSL_ACC_ODR_833Hz = 0x70, /**< ODR 833 Hz */ + LSM6DSL_ACC_ODR_1P66Hz = 0x80, /**< ODR 1.66 kHz */ + LSM6DSL_ACC_ODR_3P33Hz = 0x90, /**< ODR 3.33 kHz */ LSM6DSL_ACC_ODR_6P66Hz = 0xA0 /**< ODR 6.66 kHz */ } lsm6dsl_acc_odr_t; @@ -540,7 +540,7 @@ typedef enum { LSM6DSL_GYRO_FS_125DPS = 0x02, /**< Full scale ±125 degree per second */ LSM6DSL_GYRO_FS_250DPS = 0x00, /**< Full scale ±250 degree per second */ LSM6DSL_GYRO_FS_500DPS = 0x04, /**< Full scale ±500 degree per second */ - LSM6DSL_GYRO_FS_1000DPS = 0x08 /**< Full scale ±1000 degree per second */ + LSM6DSL_GYRO_FS_1000DPS = 0x08, /**< Full scale ±1000 degree per second */ LSM6DSL_GYRO_FS_2000DPS = 0x0C /**< Full scale ±2000 degree per second */ } lsm6dsl_gyro_fs_t; @@ -555,9 +555,9 @@ typedef enum { LSM6DSL_GYRO_ODR_104Hz = 0x40, /**< ODR 104 Hz */ LSM6DSL_GYRO_ODR_208Hz = 0x50, /**< ODR 208 Hz */ LSM6DSL_GYRO_ODR_416Hz = 0x60, /**< ODR 416 Hz */ - LSM6DSL_GYRO_ODR_833Hz = 0x70 /**< ODR 833 Hz */ - LSM6DSL_GYRO_ODR_1P66Hz = 0x80 /**< ODR 1.66 kHz */ - LSM6DSL_GYRO_ODR_3P33Hz = 0x90 /**< ODR 3.33 kHz */ + LSM6DSL_GYRO_ODR_833Hz = 0x70, /**< ODR 833 Hz */ + LSM6DSL_GYRO_ODR_1P66Hz = 0x80, /**< ODR 1.66 kHz */ + LSM6DSL_GYRO_ODR_3P33Hz = 0x90, /**< ODR 3.33 kHz */ LSM6DSL_GYRO_ODR_6P66Hz = 0xA0 /**< ODR 6.66 kHz */ } lsm6dsl_gyro_odr_t; @@ -577,7 +577,7 @@ typedef enum { LSM6DSL_GYRO_LPF_FTYPE0 = 0x00, /**< Refer to table 68 of Datasheet. */ LSM6DSL_GYRO_LPF_FTYPE1 = 0x01, /**< Refer to table 68 of Datasheet. */ LSM6DSL_GYRO_LPF_FTYPE2 = 0x10, /**< Refer to table 68 of Datasheet. */ - LSM6DSL_GYRO_LPF_FTYPE3 = 0x11, /**< Refer to table 68 of Datasheet. */ + LSM6DSL_GYRO_LPF_FTYPE3 = 0x11 /**< Refer to table 68 of Datasheet. */ } lsm6dsl_gyro_lpf_t; /** diff --git a/os/ex/ST/lsm6dsl.mk b/os/ex/ST/lsm6dsl.mk index e2bbf7b5b..e3cac3d03 100644 --- a/os/ex/ST/lsm6dsl.mk +++ b/os/ex/ST/lsm6dsl.mk @@ -1,5 +1,5 @@ # List of all the LSM6DSL device files. -LSM6DSLSRC := $(CHIBIOS)/os/ex/ST/lsm6ds0.c +LSM6DSLSRC := $(CHIBIOS)/os/ex/ST/lsm6dsl.c # Required include directories LSM6DSLINC := $(CHIBIOS)/os/hal/lib/peripherals/sensors \ -- cgit v1.2.3