Improved L3GD20 driver and related demo: improved sensitivity and bias handling.

git-svn-id: svn://svn.code.sf.net/p/chibios/svn/trunk@9819 35acf78f-673a-0410-8e92-d51de3d6d3f4

8 files changed, 100 insertions, 57 deletions

diff --git a/os/ex/ST/l3gd20.c b/os/ex/ST/l3gd20.c
index a6551b598..58e3c5f54 100644
--- a/os/ex/ST/l3gd20.c
+++ b/os/ex/ST/l3gd20.c
@@ -52,16 +52,16 @@
  *
  * @param[in] spip      pointer to the SPI interface
  * @param[in] reg       starting register address
- * @param[in] n         number of adjacent registers to write
- * @param[in] b         pointer to a buffer.
+ * @param[in] n         number of consecutive registers to read
+ * @param[in] b         pointer to an output buffer.
  */
 static void l3gd20SPIReadRegister(SPIDriver *spip, uint8_t reg,  size_t n,
-                                     uint8_t* b) {
+                                     uint8_t* rxbuf) {
   uint8_t cmd;
   (n == 1) ? (cmd = reg | L3GD20_RW) : (cmd = reg | L3GD20_RW | L3GD20_MS);
   spiSelect(spip);
   spiSend(spip, 1, &cmd);
-  spiReceive(spip, n, b);
+  spiReceive(spip, n, rxbuf);
   spiUnselect(spip);
 }
 
@@ -94,33 +94,38 @@ static size_t get_axes_number(void *ip) {
   return L3GD20_NUMBER_OF_AXES;
 }
 
-static msg_t read_raw(void *ip, int32_t axes[L3GD20_NUMBER_OF_AXES]) {
-  uint8_t buff [L3GD20_NUMBER_OF_AXES * 2], i;
+static msg_t read_raw(void *ip, int32_t axes[L3GD20_NUMBER_OF_AXES]) {

   int16_t tmp;
+  uint8_t i, buff [2 * L3GD20_NUMBER_OF_AXES];

+  msg_t msg = MSG_OK;

+  
   osalDbgCheck((ip != NULL) && (axes != NULL));
-
   osalDbgAssert((((L3GD20Driver *)ip)->state == L3GD20_READY),
               "read_raw(), invalid state");
 
 #if L3GD20_USE_SPI
   osalDbgAssert((((L3GD20Driver *)ip)->config->spip->state == SPI_READY),
-                "read_raw(), channel not ready");
+                "read_raw(), channel not ready");

+                
 #if	L3GD20_SHARED_SPI
   spiAcquireBus(((L3GD20Driver *)ip)->config->spip);
   spiStart(((L3GD20Driver *)ip)->config->spip,
            ((L3GD20Driver *)ip)->config->spicfg);
-#endif /* L3GD20_SHARED_SPI */   
+#endif /* L3GD20_SHARED_SPI */

+
     l3gd20SPIReadRegister(((L3GD20Driver *)ip)->config->spip, L3GD20_AD_OUT_X_L,
-                          L3GD20_NUMBER_OF_AXES * 2, buff);
+                          L3GD20_NUMBER_OF_AXES * 2, buff);

+

+#if	L3GD20_SHARED_SPI

+  spiReleaseBus(((L3GD20Driver *)ip)->config->spip);

+#endif /* L3GD20_SHARED_SPI */   

+#endif /* L3GD20_USE_SPI */ 

+
     for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++) {
       tmp = buff[2*i] + (buff[2*i+1] << 8);
       axes[i] = (int32_t)tmp;
     }
-#if	L3GD20_SHARED_SPI
-  spiReleaseBus(((L3GD20Driver *)ip)->config->spip);
-#endif /* L3GD20_SHARED_SPI */   
-#endif /* L3GD20_USE_SPI */ 
-  return MSG_OK;
+  return msg;
 }
 
 static msg_t read_cooked(void *ip, float axes[]) {
@@ -135,7 +140,7 @@ static msg_t read_cooked(void *ip, float axes[]) {
 
   msg = read_raw(ip, raw);
   for(i = 0; i < L3GD20_NUMBER_OF_AXES ; i++){
-    axes[i] = (raw[i] * ((L3GD20Driver *)ip)->sensitivity[i]);
+    axes[i] = raw[i] * ((L3GD20Driver *)ip)->sensitivity[i];
     axes[i] -= ((L3GD20Driver *)ip)->bias[i];
   }
   return msg;
@@ -257,6 +262,7 @@ static msg_t set_full_scale(void *ip, l3gd20_fs_t fs) {
     scale = newfs / ((L3GD20Driver *)ip)->fullscale;
     ((L3GD20Driver *)ip)->fullscale = newfs;

 

+#if L3GD20_USE_SPI

 #if	L3GD20_SHARED_SPI

 		spiAcquireBus(((L3GD20Driver *)ip)->config->spip);

 		spiStart(((L3GD20Driver *)ip)->config->spip,

@@ -266,13 +272,27 @@ static msg_t set_full_scale(void *ip, l3gd20_fs_t fs) {
     /* Updating register.*/

     l3gd20SPIReadRegister(((L3GD20Driver *)ip)->config->spip,

                           L3GD20_AD_CTRL_REG4, 1, &cr);

+

+#if	L3GD20_SHARED_SPI

+		spiReleaseBus(((L3GD20Driver *)ip)->config->spip);

+#endif /* L3GD20_SHARED_SPI */ 

+#endif /* L3GD20_USE_SPI */

     cr &= ~(L3GD20_CTRL_REG4_FS_MASK);

     cr |= fs;

+ 

+#if L3GD20_USE_SPI 

+#if	L3GD20_SHARED_SPI

+		spiAcquireBus(((L3GD20Driver *)ip)->config->spip);

+		spiStart(((L3GD20Driver *)ip)->config->spip,

+						 ((L3GD20Driver *)ip)->config->spicfg);

+#endif /* L3GD20_SHARED_SPI */ 

+

     l3gd20SPIWriteRegister(((L3GD20Driver *)ip)->config->spip,

                            L3GD20_AD_CTRL_REG4, 1, &cr);

 #if	L3GD20_SHARED_SPI

 		spiReleaseBus(((L3GD20Driver *)ip)->config->spip);

 #endif /* L3GD20_SHARED_SPI */ 

+#endif /* L3GD20_USE_SPI */

 
     /* Scaling sensitivity and bias. Re-calibration is suggested anyway. */
     for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++) {
@@ -318,7 +338,7 @@ void l3gd20ObjectInit(L3GD20Driver *devp) {
   devp->vmt_l3gd20 = &vmt_l3gd20;
   devp->config = NULL;
   for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++)
-    devp->bias[i] = 0;
+    devp->bias[i] = 0.0f;
   devp->state  = L3GD20_STOP;
 }
 
@@ -339,14 +359,7 @@ void l3gd20Start(L3GD20Driver *devp, const L3GD20Config *config) {
               "l3gd20Start(), invalid state");
 
   devp->config = config;
-  
-#if L3GD20_USE_SPI
-#if	L3GD20_SHARED_SPI
-  spiAcquireBus((devp)->config->spip);
-#endif /* L3GD20_SHARED_SPI */
-  spiStart((devp)->config->spip,
-           (devp)->config->spicfg);
-           
+             
   /* Control register 1 configuration block.*/
   {
     cr[0] = L3GD20_CTRL_REG1_XEN | L3GD20_CTRL_REG1_YEN | 
@@ -390,7 +403,15 @@ void l3gd20Start(L3GD20Driver *devp, const L3GD20Config *config) {
     }
   }
 #endif
-  }  
+  } 

+

+#if L3GD20_USE_SPI

+#if	L3GD20_SHARED_SPI

+  spiAcquireBus((devp)->config->spip);

+#endif /* L3GD20_SHARED_SPI */

+  spiStart((devp)->config->spip,

+           (devp)->config->spicfg);

+           
   l3gd20SPIWriteRegister(devp->config->spip, L3GD20_AD_CTRL_REG1, 
                          5, cr);
 #if	L3GD20_SHARED_SPI
@@ -401,21 +422,40 @@ void l3gd20Start(L3GD20Driver *devp, const L3GD20Config *config) {
   /* Storing sensitivity information according to full scale.*/
   if(devp->config->fullscale == L3GD20_FS_250DPS) {
     devp->fullscale = L3GD20_250DPS;
-    for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++)
-      devp->sensitivity[i] = L3GD20_SENS_250DPS;
+    for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++) {

+      if (devp->config->sensitivity == NULL)

+        devp->sensitivity[i] = L3GD20_SENS_250DPS;

+      else

+        devp->sensitivity[i] = devp->config->sensitivity[i];

+    }
   }
   else if(devp->config->fullscale == L3GD20_FS_500DPS) {
     devp->fullscale = L3GD20_500DPS;
-    for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++)
-      devp->sensitivity[i] = L3GD20_SENS_500DPS;
+    for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++) {
+      if (devp->config->sensitivity == NULL)

+        devp->sensitivity[i] = L3GD20_SENS_500DPS;

+      else

+        devp->sensitivity[i] = devp->config->sensitivity[i];

+    }
   }
   else if(devp->config->fullscale == L3GD20_FS_2000DPS) {
     devp->fullscale = L3GD20_2000DPS;
-    for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++)
-      devp->sensitivity[i] = L3GD20_SENS_2000DPS;
+    for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++) {
+      if (devp->config->sensitivity == NULL)

+        devp->sensitivity[i] = L3GD20_SENS_2000DPS;

+      else

+        devp->sensitivity[i] = devp->config->sensitivity[i];

+    }
   }
   else
-    osalDbgAssert(FALSE, "l3gd20Start(), full scale issue");
+    osalDbgAssert(FALSE, "l3gd20Start(), full scale issue");

+  

+  if(devp->config->bias != NULL) {

+    for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++) {

+      devp->bias[i] = devp->config->bias[i];

+    }

+  }

+  
   /* This is the Gyroscope transient recovery time.*/
   osalThreadSleepMilliseconds(10);
 
@@ -435,24 +475,27 @@ void l3gd20Stop(L3GD20Driver *devp) {
 
   osalDbgAssert((devp->state == L3GD20_STOP) || (devp->state == L3GD20_READY),
                 "l3gd20Stop(), invalid state");
-
-#if (L3GD20_USE_SPI)
-  if (devp->state == L3GD20_STOP) {
+

+  if (devp->state == L3GD20_READY) {

+    /* Disabling all axes and enabling power down mode.*/

+    cr1 = 0;

+    
+#if L3GD20_USE_SPI
 #if	L3GD20_SHARED_SPI
     spiAcquireBus((devp)->config->spip);
     spiStart((devp)->config->spip,
              (devp)->config->spicfg);
 #endif /* L3GD20_SHARED_SPI */
-    /* Disabling all axes and enabling power down mode.*/
-    cr1 = 0;
+
     l3gd20SPIWriteRegister(devp->config->spip, L3GD20_AD_CTRL_REG1, 
                            1, &cr1);
-    spiStop((devp)->config->spip);
+    spiStop((devp)->config->spip);

+    
 #if	L3GD20_SHARED_SPI
     spiReleaseBus((devp)->config->spip);
-#endif /* L3GD20_SHARED_SPI */
-  }			  
-#endif /* L3GD20_USE_SPI */
+#endif /* L3GD20_SHARED_SPI */ 
+#endif /* L3GD20_USE_SPI */

+  }			 
   devp->state = L3GD20_STOP;
 }
 /** @} */
diff --git a/os/ex/ST/l3gd20.h b/os/ex/ST/l3gd20.h
index 7dfce1f82..d658aef0c 100644
--- a/os/ex/ST/l3gd20.h
+++ b/os/ex/ST/l3gd20.h
@@ -40,7 +40,7 @@
 /**

  * @brief   L3GD20 driver version string.

  */

-#define EX_L3GD20_VERSION           "1.0.3"

+#define EX_L3GD20_VERSION           "1.0.4"

 

 /**

  * @brief   L3GD20 driver version major number.

@@ -55,7 +55,7 @@
 /**

  * @brief   L3GD20 driver version patch number.

  */

-#define EX_L3GD20_PATCH             3

+#define EX_L3GD20_PATCH             4

 /** @} */

 

 /**

@@ -429,12 +429,12 @@ typedef struct {
   /**
    * @brief L3GD20 initial sensitivity.
    */
-  float                     sensitivity[L3GD20_NUMBER_OF_AXES];
+  float                     *sensitivity;
   /**
    * @brief L3GD20 initial bias.
    */
-  float                     bias[L3GD20_NUMBER_OF_AXES];

-  /**
+  float                     *bias;

+  /** 
    * @brief L3GD20 initial full scale value.
    */
   l3gd20_fs_t               fullscale;
diff --git a/testhal/STM32/STM32F3xx/SPI-L3GD20/Makefile b/testhal/STM32/STM32F3xx/SPI-L3GD20/Makefile
index 3679b008a..ba5a5012e 100644
--- a/testhal/STM32/STM32F3xx/SPI-L3GD20/Makefile
+++ b/testhal/STM32/STM32F3xx/SPI-L3GD20/Makefile
@@ -201,7 +201,7 @@ CPPWARN = -Wall -Wextra -Wundef
 

 # List all user C define here, like -D_DEBUG=1

 UDEFS = -DCHPRINTF_USE_FLOAT=1 -DSHELL_CMD_TEST_ENABLED=0 \

-        -DL3GD20_USE_ADVANCED=0

+        -DL3GD20_USE_ADVANCED=0 -DL3GD20_SHARED_SPI=0

 

 # Define ASM defines here

 UADEFS =

diff --git a/testhal/STM32/STM32F3xx/SPI-L3GD20/main.c b/testhal/STM32/STM32F3xx/SPI-L3GD20/main.c
index fb21b0af8..2df0437b1 100644
--- a/testhal/STM32/STM32F3xx/SPI-L3GD20/main.c
+++ b/testhal/STM32/STM32F3xx/SPI-L3GD20/main.c
@@ -48,8 +48,8 @@ static const SPIConfig spicfg = {
 static L3GD20Config l3gd20cfg = {

   &SPID1,                                    /* Pointer to SPI Driver.*/

   &spicfg,                                   /* Pointer to SPI Configuration.*/

-  {0, 0, 0},                                 /* Use default sensitivity.*/

-  {0, 0, 0},                                 /* Use default bias.*/

+  NULL,                                      /* Use default sensitivity.*/

+  NULL,                                      /* Use default bias.*/

   L3GD20_FS_250DPS,                          /* Full scale value.*/

   L3GD20_ODR_760HZ,                          /* Output data rate.*/

 #if L3GD20_USE_ADVANCED || defined(__DOXYGEN__)

diff --git a/testhal/STM32/STM32F4xx/SPI-L3GD20/Makefile b/testhal/STM32/STM32F4xx/SPI-L3GD20/Makefile
index db92c1e2c..655a35ddb 100644
--- a/testhal/STM32/STM32F4xx/SPI-L3GD20/Makefile
+++ b/testhal/STM32/STM32F4xx/SPI-L3GD20/Makefile
@@ -201,7 +201,7 @@ CPPWARN = -Wall -Wextra -Wundef
 

 # List all user C define here, like -D_DEBUG=1

 UDEFS = -DCHPRINTF_USE_FLOAT=1 -DSHELL_CMD_TEST_ENABLED=0 \

-        -DL3GD20_USE_ADVANCED=0

+        -DL3GD20_USE_ADVANCED=0 -DL3GD20_SHARED_SPI=0

 

 # Define ASM defines here

 UADEFS =

diff --git a/testhal/STM32/STM32F4xx/SPI-L3GD20/main.c b/testhal/STM32/STM32F4xx/SPI-L3GD20/main.c
index ae30c6c98..2019b99a3 100644
--- a/testhal/STM32/STM32F4xx/SPI-L3GD20/main.c
+++ b/testhal/STM32/STM32F4xx/SPI-L3GD20/main.c
@@ -48,8 +48,8 @@ static const SPIConfig spicfg = {
 static L3GD20Config l3gd20cfg = {

   &SPID1,                                    /* Pointer to SPI Driver.*/

   &spicfg,                                   /* Pointer to SPI Configuration.*/

-  {0, 0, 0},                                 /* Use default sensitivity.*/

-  {0, 0, 0},                                 /* Use default bias.*/

+  NULL,                                      /* Use default sensitivity.*/

+  NULL,                                      /* Use default bias.*/

   L3GD20_FS_250DPS,                          /* Full scale value.*/

   L3GD20_ODR_760HZ,                          /* Output data rate.*/

 #if L3GD20_USE_ADVANCED || defined(__DOXYGEN__)

diff --git a/testhal/STM32/STM32L4xx/SPI-L3GD20/Makefile b/testhal/STM32/STM32L4xx/SPI-L3GD20/Makefile
index 4f6779d7b..aef87d496 100644
--- a/testhal/STM32/STM32L4xx/SPI-L3GD20/Makefile
+++ b/testhal/STM32/STM32L4xx/SPI-L3GD20/Makefile
@@ -5,7 +5,7 @@
 

 # Compiler options here.

 ifeq ($(USE_OPT),)

-  USE_OPT = -O2 -ggdb -fomit-frame-pointer -falign-functions=16

+  USE_OPT = -O0 -ggdb -fomit-frame-pointer -falign-functions=16

 endif

 

 # C specific options here (added to USE_OPT).

@@ -201,7 +201,7 @@ CPPWARN = -Wall -Wextra -Wundef
 

 # List all user C define here, like -D_DEBUG=1

 UDEFS = -DCHPRINTF_USE_FLOAT=1 -DSHELL_CMD_TEST_ENABLED=0 \

-        -DL3GD20_USE_ADVANCED=0

+        -DL3GD20_USE_ADVANCED=0 -DL3GD20_SHARED_SPI=0

 

 # Define ASM defines here

 UADEFS =

diff --git a/testhal/STM32/STM32L4xx/SPI-L3GD20/main.c b/testhal/STM32/STM32L4xx/SPI-L3GD20/main.c
index 644a155f6..ef9367137 100644
--- a/testhal/STM32/STM32L4xx/SPI-L3GD20/main.c
+++ b/testhal/STM32/STM32L4xx/SPI-L3GD20/main.c
@@ -47,8 +47,8 @@ static const SPIConfig spicfg = {
 static L3GD20Config l3gd20cfg = {

   &SPID2,                                    /* Pointer to SPI Driver.*/

   &spicfg,                                   /* Pointer to SPI Configuration.*/

-  {0, 0, 0},                                 /* Use default sensitivity.*/

-  {0, 0, 0},                                 /* Use default bias.*/

+  NULL,                                      /* Use default sensitivity.*/

+  NULL,                                      /* Use default bias.*/

   L3GD20_FS_250DPS,                          /* Full scale value.*/

   L3GD20_ODR_760HZ,                          /* Output data rate.*/

 #if L3GD20_USE_ADVANCED || defined(__DOXYGEN__)