From 9f054efd67930f2dd6ef2a2f4e28eea3d8b10431 Mon Sep 17 00:00:00 2001 From: patacongo Date: Tue, 5 Apr 2011 16:25:04 +0000 Subject: LIS331DL and STM32 I2C patch from Uros git-svn-id: svn://svn.code.sf.net/p/nuttx/code/trunk@3467 42af7a65-404d-4744-a932-0658087f49c3 --- nuttx/arch/arm/src/stm32/stm32_i2c.c | 52 ++++++++++++++++++++++++++--------- nuttx/configs/vsn/src/sif.c | 9 +++--- nuttx/drivers/i2c/st_lis331dl.c | 49 ++++++++++++++++++++------------- nuttx/include/nuttx/i2c/st_lis331dl.h | 18 +++++++++--- 4 files changed, 88 insertions(+), 40 deletions(-) (limited to 'nuttx') diff --git a/nuttx/arch/arm/src/stm32/stm32_i2c.c b/nuttx/arch/arm/src/stm32/stm32_i2c.c index de9ca60c8..dbbc11d5f 100644 --- a/nuttx/arch/arm/src/stm32/stm32_i2c.c +++ b/nuttx/arch/arm/src/stm32/stm32_i2c.c @@ -89,6 +89,7 @@ * Private Types ************************************************************************************/ +#define I2C_FLAGS 0x8000 /* RxNE and TxE enabled */ /** I2C Device Private Data */ @@ -263,6 +264,7 @@ static inline uint32_t stm32_i2c_getstatus(FAR struct stm32_i2c_priv_s *priv) /************************************************************************************ * Interrupt Service Routines ************************************************************************************/ + //DEBUG TO BE CLEANED //#define NON_ISR @@ -273,9 +275,8 @@ static int stm32_i2c_isr(struct stm32_i2c_priv_s * priv) #ifdef NON_ISR static uint32_t isr_count = 0; static uint32_t old_status = 0xFFFF; - isr_count++; - + if (old_status != status) { printf("status = %8x, count=%d\n", status, isr_count); fflush(stdout); old_status = status; @@ -298,10 +299,12 @@ static int stm32_i2c_isr(struct stm32_i2c_priv_s * priv) 0 : ((priv->msgv->addr << 1) | (priv->flags & I2C_M_READ)) ); + + /* Set ACK for receive mode */ - /* Enable RxNE and TxE buffers */ - - stm32_i2c_modifyreg(priv, STM32_I2C_CR2_OFFSET, 0, I2C_CR2_ITBUFEN); + if (priv->dcnt > 1 && (priv->flags & I2C_M_READ) ) { + stm32_i2c_modifyreg(priv, STM32_I2C_CR1_OFFSET, 0, I2C_CR1_ACK); + } /* Increment to next pointer and decrement message count */ @@ -313,28 +316,27 @@ static int stm32_i2c_isr(struct stm32_i2c_priv_s * priv) else if (status & I2C_SR1_ADD10) { /* \todo Finish 10-bit mode addressing */ + } /* Was address sent, continue with ether sending or reading data */ else if ( !(priv->flags & I2C_M_READ) && (status & (I2C_SR1_ADDR | I2C_SR1_TXE)) ) { - + if (--priv->dcnt >= 0) { /* Send a byte */ #ifdef NON_ISR printf("Send byte: %2x\n", *priv->ptr); #endif - stm32_i2c_putreg(priv, STM32_I2C_DR_OFFSET, *priv->ptr++); + stm32_i2c_putreg(priv, STM32_I2C_DR_OFFSET, *priv->ptr++); } } else if ( (priv->flags & I2C_M_READ) && (status & I2C_SR1_ADDR) ) { - /* Acknowledge bytes if there is more than one to be received */ + /* Enable RxNE and TxE buffers in order to receive one or multiple bytes */ - if (priv->dcnt > 1) { - stm32_i2c_modifyreg(priv, STM32_I2C_CR1_OFFSET, 0, I2C_CR1_ACK); - } + stm32_i2c_modifyreg(priv, STM32_I2C_CR2_OFFSET, 0, I2C_CR2_ITBUFEN); } /* More bytes to read */ @@ -356,8 +358,20 @@ static int stm32_i2c_isr(struct stm32_i2c_priv_s * priv) stm32_i2c_modifyreg(priv, STM32_I2C_CR1_OFFSET, I2C_CR1_ACK, 0); } } + } + /* Do we have more bytes to send, enable/disable buffer interrupts + * (these ISRs could be replaced by DMAs) + */ + + if (priv->dcnt>0) { + stm32_i2c_modifyreg(priv, STM32_I2C_CR2_OFFSET, 0, I2C_CR2_ITBUFEN); + } + else if (priv->dcnt==0 ) { + stm32_i2c_modifyreg(priv, STM32_I2C_CR2_OFFSET, I2C_CR2_ITBUFEN, 0); + } + /* Was last byte received or sent? */ @@ -383,6 +397,7 @@ static int stm32_i2c_isr(struct stm32_i2c_priv_s * priv) priv->flags = priv->msgv->flags; priv->msgv++; priv->msgc--; + stm32_i2c_modifyreg(priv, STM32_I2C_CR2_OFFSET, 0, I2C_CR2_ITBUFEN); // Restart this ISR! } else { stm32_i2c_sendstart(priv); @@ -392,7 +407,6 @@ static int stm32_i2c_isr(struct stm32_i2c_priv_s * priv) #ifdef NON_ISR printf("stop2: status = %8x\n", status); #endif - stm32_i2c_modifyreg(priv, STM32_I2C_CR2_OFFSET, I2C_CR2_ITBUFEN, 0); stm32_i2c_sendstop(priv); sem_post( &priv->sem_isr ); priv->msgv = NULL; /* mark that we have stopped with this transaction */ @@ -596,6 +610,9 @@ int stm32_i2c_process(FAR struct i2c_dev_s *dev, FAR struct i2c_msg_s *msgs, int * found in the local status variable. */ +// inst->priv->status = 0; +// printf("isr_count = %d\n", isr_count); fflush(stdout); + stm32_i2c_sendstart(inst->priv); #ifdef NON_ISR @@ -607,13 +624,20 @@ int stm32_i2c_process(FAR struct i2c_dev_s *dev, FAR struct i2c_msg_s *msgs, int } while( sem_trywait( &((struct stm32_i2c_inst_s *)dev)->priv->sem_isr ) != 0 ); #else +#if 1 /* Wait for an ISR, if there was a timeout, fetch latest status to get the BUSY flag */ if (stm32_i2c_sem_waitisr(dev) == ERROR) { status = stm32_i2c_getstatus(inst->priv); } else status = inst->priv->status & 0xFFFF; /* clear SR2 (BUSY flag) as we've done successfully */ -#endif +#else + do { + printf("%x, %d\n", inst->priv->status, isr_count ); + } + while( sem_trywait( &inst->priv->sem_isr ) != 0 ); +#endif +#endif if (status & I2C_SR1_BERR) { /* Bus Error */ status_errno = EIO; @@ -637,6 +661,8 @@ int stm32_i2c_process(FAR struct i2c_dev_s *dev, FAR struct i2c_msg_s *msgs, int status_errno = EBUSY; } +// printf("end_count = %d, dcnt=%d\n", isr_count, inst->priv->dcnt); fflush(stdout); + stm32_i2c_sem_post(dev); errno = status_errno; diff --git a/nuttx/configs/vsn/src/sif.c b/nuttx/configs/vsn/src/sif.c index a10072ac5..6f75c4a9e 100644 --- a/nuttx/configs/vsn/src/sif.c +++ b/nuttx/configs/vsn/src/sif.c @@ -512,15 +512,16 @@ int sif_main(int argc, char *argv[]) /* Sample some values */ - for (i=0; i<20; i++) { - if ( (a = st_lis331dl_getreadings(lis)) ) + for (i=0; i<200000; i++) { + if ( (a = st_lis331dl_getreadings(lis)) ) { printf("%d %d %d\n", a->x, a->y, a->z); - else { + } + else if (errno != 11) { printf("Readings errno %d\n", errno); break; } fflush(stdout); - usleep(100000); + usleep(10000); } st_lis331dl_deinit(lis); diff --git a/nuttx/drivers/i2c/st_lis331dl.c b/nuttx/drivers/i2c/st_lis331dl.c index cb1d7f47d..ca2646484 100644 --- a/nuttx/drivers/i2c/st_lis331dl.c +++ b/nuttx/drivers/i2c/st_lis331dl.c @@ -107,7 +107,7 @@ int st_lis331dl_access(struct st_lis331dl_dev_s * dev, uint8_t subaddr, uint8_t return ERROR; } - subaddr |= 0x80; + if (length > 1) subaddr |= 0x80; /* Create message and send */ @@ -135,9 +135,7 @@ int st_lis331dl_access(struct st_lis331dl_dev_s * dev, uint8_t subaddr, uint8_t int st_lis331dl_readregs(struct st_lis331dl_dev_s * dev) { - if (st_lis331dl_access(dev, ST_LIS331DL_CTRL_REG1, &dev->cr1, 3) == ERROR) return ERROR; - - printf("CR1=%2x, CR2=%2x, CR3=%2x\n", dev->cr1, dev->cr2, dev->cr3 ); + if (st_lis331dl_access(dev, ST_LIS331DL_CTRL_REG1, &dev->cr1, 3) != 3) return ERROR; return OK; } @@ -173,7 +171,7 @@ struct st_lis331dl_dev_s * st_lis331dl_init(struct i2c_dev_s * i2c, uint16_t add /* Copy LIS331DL registers to our private structure and power-up device */ - if ( st_lis331dl_readregs(dev)==OK && st_lis331dl_powerup(dev)==OK) { + if (st_lis331dl_readregs(dev)==OK && st_lis331dl_powerup(dev)==OK) { /* Normal exit point */ errno = 0; @@ -201,7 +199,7 @@ int st_lis331dl_deinit(struct st_lis331dl_dev_s * dev) { ASSERT(dev); -// st_lis331dl_powerdown(dev); + st_lis331dl_powerdown(dev); free(dev); return OK; @@ -212,17 +210,22 @@ int st_lis331dl_powerup(struct st_lis331dl_dev_s * dev) { dev->cr1 = ST_LIS331DL_CR1_PD | ST_LIS331DL_CR1_ZEN | ST_LIS331DL_CR1_YEN | ST_LIS331DL_CR1_XEN; + dev->cr2 = 0; + dev->cr3 = 0; - st_lis331dl_access(dev, ST_LIS331DL_CTRL_REG1, &dev->cr1, -1); - return OK; + if (st_lis331dl_access(dev, ST_LIS331DL_CTRL_REG1, &dev->cr1, -3) == 3) return OK; + return ERROR; } int st_lis331dl_powerdown(struct st_lis331dl_dev_s * dev) { dev->cr1 = ST_LIS331DL_CR1_ZEN | ST_LIS331DL_CR1_YEN | ST_LIS331DL_CR1_XEN; + dev->cr2 = 0; + dev->cr3 = 0; - return st_lis331dl_access(dev, ST_LIS331DL_CTRL_REG1, &dev->cr1, -1); + if (st_lis331dl_access(dev, ST_LIS331DL_CTRL_REG1, &dev->cr1, -3) == 3) return OK; + return ERROR; } @@ -232,16 +235,16 @@ int st_lis331dl_setconversion(struct st_lis331dl_dev_s * dev, bool full, bool fa (full ? ST_LIS331DL_CR1_FS : 0) | (fast ? ST_LIS331DL_CR1_DR : 0) | ST_LIS331DL_CR1_ZEN | ST_LIS331DL_CR1_YEN | ST_LIS331DL_CR1_XEN; - st_lis331dl_access(dev, ST_LIS331DL_CTRL_REG1, &dev->cr1, -1); - return OK; + if (st_lis331dl_access(dev, ST_LIS331DL_CTRL_REG1, &dev->cr1, -1) == 1) return OK; + return ERROR; } -float st_lis331dl_getprecision(struct st_lis331dl_dev_s * dev) +int st_lis331dl_getprecision(struct st_lis331dl_dev_s * dev) { if (dev->cr1 & ST_LIS331DL_CR1_FS) - return 9.0/127.0; /* ~9g full scale */ - return 2.0/127.0; /* ~2g full scale */ + return 9200/127; /* typ. 9.2g full scale */ + return 2300/127; /* typ. 2.3g full scale */ } @@ -255,14 +258,22 @@ int st_lis331dl_getsamplerate(struct st_lis331dl_dev_s * dev) const struct st_lis331dl_vector_s * st_lis331dl_getreadings(struct st_lis331dl_dev_s * dev) { - uint8_t retval[5]; + uint8_t retval[7]; ASSERT(dev); - if (st_lis331dl_access(dev, ST_LIS331DL_OUT_X, retval, 5) == 5) { - dev->a.x = retval[0]; - dev->a.y = retval[2]; - dev->a.z = retval[4]; + if (st_lis331dl_access(dev, ST_LIS331DL_STATUS_REG, retval, 7) == 7) { + + /* If result is not yet ready, return NULL */ + + if ( !(retval[0] & ST_LIS331DL_SR_ZYXDA) ) { + errno = EAGAIN; + return NULL; + } + + dev->a.x = retval[2]; + dev->a.y = retval[4]; + dev->a.z = retval[6]; return &dev->a; } diff --git a/nuttx/include/nuttx/i2c/st_lis331dl.h b/nuttx/include/nuttx/i2c/st_lis331dl.h index f3f137391..31092ae79 100644 --- a/nuttx/include/nuttx/i2c/st_lis331dl.h +++ b/nuttx/include/nuttx/i2c/st_lis331dl.h @@ -81,7 +81,15 @@ extern "C" { #define ST_LIS331DL_HP_FILTER_RESET 0x23 -#define ST_LIS331DL_STATUS_REG 0x27 +#define ST_LIS331DL_STATUS_REG 0x27 /* Status Register */ +#define ST_LIS331DL_SR_ZYXOR 0x80 /* OR'ed X,Y and Z data over-run */ +#define ST_LIS331DL_SR_ZOR 0x40 /* individual data over-run ... */ +#define ST_LIS331DL_SR_YOR 0x20 +#define ST_LIS331DL_SR_XOR 0x10 +#define ST_LIS331DL_SR_ZYXDA 0x08 /* OR'ed X,Y and Z data available */ +#define ST_LIS331DL_SR_ZDA 0x04 /* individual data available ... */ +#define ST_LIS331DL_SR_YDA 0x02 +#define ST_LIS331DL_SR_XDA 0x01 #define ST_LIS331DL_OUT_X 0x29 #define ST_LIS331DL_OUT_Y 0x2B @@ -141,9 +149,9 @@ EXTERN int st_lis331dl_setconversion(struct st_lis331dl_dev_s * dev, bool full, /** Get precision * - * \return Precision of 1 LSB in terms of unit [g] + * \return Precision of 1 LSB in terms of unit [mg] **/ -EXTERN float st_lis331dl_getprecision(struct st_lis331dl_dev_s * dev); +EXTERN int st_lis331dl_getprecision(struct st_lis331dl_dev_s * dev); /** Get sample rate * @@ -154,7 +162,9 @@ EXTERN int st_lis331dl_getsamplerate(struct st_lis331dl_dev_s * dev); /** Get readings, updates internal data structure * * \param dev Device to LIS331DL device structure - * \return Ptr to vector acceleration [x,y,z] on success, or NULL on error with errno set + * \return Ptr to vector acceleration [x,y,z] on success, or NULL on error with errno set. + * If data is not yet ready to be read from the LIS331 then errno is set to EAGAIN otherwise + * errno is set by I2C_TRANSFER(). */ EXTERN const struct st_lis331dl_vector_s * st_lis331dl_getreadings(struct st_lis331dl_dev_s * dev); -- cgit v1.2.3