EX: added get temperature for L3GD20, improved related demos

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

5 files changed, 57 insertions, 55 deletions

diff --git a/os/ex/ST/l3gd20.c b/os/ex/ST/l3gd20.c
index 2fa329a76..2b4e2cc22 100644
--- a/os/ex/ST/l3gd20.c
+++ b/os/ex/ST/l3gd20.c
@@ -336,16 +336,12 @@ static msg_t reset_sensivity(void *ip) {
   return MSG_OK;
 }
 
-static msg_t enable_temperature_compensation(void *ip) {
-  (void) ip;
-  /* TODO complete this function */
-  return 0;
-}
+static msg_t get_temperature(void *ip, float* tempp) {
+
+  *tempp = (int8_t)l3gd20SPIReadRegister(((L3GD20Driver *)ip)->config->spip,
+                                       L3GD20_AD_OUT_TEMP);
 
-static msg_t disable_temperature_compensation(void *ip) {
-  (void) ip;
-  /* TODO complete this function */
-  return 0;
+  return MSG_OK;
 }
 
 static const struct BaseSensorVMT vmt_basesensor = {
@@ -355,9 +351,13 @@ static const struct BaseSensorVMT vmt_basesensor = {
 static const struct BaseGyroscopeVMT vmt_basegyroscope = {
   get_axes_number, read_raw, read_cooked,
   sample_bias, set_bias, reset_bias,
-  set_sensivity, reset_sensivity,
-  enable_temperature_compensation,
-  disable_temperature_compensation
+  set_sensivity, reset_sensivity
+};
+
+static const struct L3GD20VMT vmt_l3gd20 = {
+  get_axes_number, read_raw, read_cooked,
+  sample_bias, set_bias, reset_bias,
+  set_sensivity, reset_sensivity, get_temperature
 };
 
 /*===========================================================================*/
@@ -376,7 +376,7 @@ void l3gd20ObjectInit(L3GD20Driver *devp) {
 
   devp->vmt_basesensor = &vmt_basesensor;
   devp->vmt_basegyroscope = &vmt_basegyroscope;
-  devp->vmt = (struct L3GD20VMT*) &vmt_basegyroscope;
+  devp->vmt_l3gd20 = &vmt_l3gd20;
   devp->state  = L3GD20_STOP;
   devp->config = NULL;
   for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++)
diff --git a/os/ex/ST/l3gd20.h b/os/ex/ST/l3gd20.h
index e13dafba5..ddd21a543 100644
--- a/os/ex/ST/l3gd20.h
+++ b/os/ex/ST/l3gd20.h
@@ -245,7 +245,10 @@ typedef struct L3GD20Driver L3GD20Driver;
  * @brief   @p L3GD20 specific methods.
  */
 #define _l3gd20_methods                                                     \
-  _base_gyroscope_methods
+  _base_gyroscope_methods                                                   \
+  /* Retrieve the temperature of L3GD20 chip.*/                             \
+  msg_t (*get_temperature)(void *instance, float* temperature);
+
 
 /**
  * @extends BaseGyroscopeVMT
@@ -278,12 +281,14 @@ struct L3GD20VMT {
  *          driver implementation.
  */
 struct L3GD20Driver {
-  /** @brief Virtual Methods Table.*/
+  /** @brief Base Sensor Virtual Methods Table.*/
   const struct BaseSensorVMT *vmt_basesensor;
-  /** @brief Virtual Methods Table.*/
+  /** @brief Base Gyroscope Virtual Methods Table.
+    * @note  Extend BaseSensor VMT.*/
   const struct BaseGyroscopeVMT *vmt_basegyroscope;
-  /** @brief Virtual Methods Table.*/
-  const struct L3GD20VMT *vmt;
+  /** @brief L3GD20 Virtual Methods Table.
+   * @note  Extend BaseGyroscope VMT.*/
+  const struct L3GD20VMT *vmt_l3gd20;
   _l3gd20_data
 };
 /** @} */
@@ -292,6 +297,21 @@ struct L3GD20Driver {
 /* Driver macros.                                                            */
 /*===========================================================================*/
 
+/**
+ * @brief   Get current MEMS temperature.
+ * @detail  This information is very useful especially for high accuracy IMU
+ *
+ * @param[in] ip        pointer to a @p BaseGyroscope class.
+ * @param[out] temp     the MEMS temperature as single precision floating.
+ *
+ * @return              The operation status.
+ * @retval MSG_OK       if the function succeeded.
+ * @retval MSG_RESET    if one or more errors occurred.
+ * @api
+ */
+#define gyroscopeGetTemp(ip, tpp)                                           \
+        (ip)->vmt_l3gd20->get_temperature(ip, tpp)
+
 /*===========================================================================*/
 /* External declarations.                                                    */
 /*===========================================================================*/
diff --git a/os/hal/lib/peripherals/sensors/hal_gyroscope.h b/os/hal/lib/peripherals/sensors/hal_gyroscope.h
index 9ffb1b9f5..1fb36e8d8 100644
--- a/os/hal/lib/peripherals/sensors/hal_gyroscope.h
+++ b/os/hal/lib/peripherals/sensors/hal_gyroscope.h
@@ -56,11 +56,7 @@
   /* Invoke the set sensitivity procedure.*/                                \

   msg_t (*set_sensitivity)(void *instance, float sensitivities[]);          \

   /* Restore sensitivity stored data to default.*/                          \

-  msg_t (*reset_sensitivity)(void *instance);                               \

-  /* Enable temperature drift effect compensation.*/                        \

-  msg_t (*enable_temperature_compensation)(void *instance);                 \

-  /* Disable temperature drift effect compensation.*/                       \

-  msg_t (*disable_temperature_compensation)(void *instance);                                 

+  msg_t (*reset_sensitivity)(void *instance);

   

   

 /**

@@ -221,34 +217,6 @@ typedef struct {
  */

 #define gyroscopeResetSensitivity(ip)                                       \

         (ip)->vmt_basegyroscope->reset_sensitivity(ip)

-

-/**

- * @brief   Enables data compensation removing temperature drift.

- *

- * @param[in] ip        pointer to a @p BaseGyroscope class.

- * 

- * @return              The operation status.

- * @retval MSG_OK       if the function succeeded.

- * @retval MSG_RESET    if one or more errors occurred.

- *

- * @api

- */

-#define gyroscopeEnableTempCompensation(ip)                                 \

-        (ip)->vmt_basegyroscope->enable_temperature_compensation(ip)		

-

-/**

- * @brief   Disable data compensation.

- *

- * @param[in] ip        pointer to a @p BaseGyroscope class.

- * 

- * @return              The operation status.

- * @retval MSG_OK       if the function succeeded.

- * @retval MSG_RESET    if one or more errors occurred.

- *

- * @api

- */

-#define gyroscopeDisableTempCompensation(ip)                                 \

-        (ip)->vmt_basegyroscope->disable_temperature_compensation(ip)			

 /** @} */

 

 /*===========================================================================*/

diff --git a/testhal/STM32/STM32F3xx/SPI-L3GD20/main.c b/testhal/STM32/STM32F3xx/SPI-L3GD20/main.c
index 31ec3004f..a845f92a7 100644
--- a/testhal/STM32/STM32F3xx/SPI-L3GD20/main.c
+++ b/testhal/STM32/STM32F3xx/SPI-L3GD20/main.c
@@ -38,6 +38,7 @@ static L3GD20Driver L3GD20D1;
 

 static int32_t rawdata[L3GD20_NUMBER_OF_AXES];

 static float cookeddata[L3GD20_NUMBER_OF_AXES];

+static float temperature;

 

 static char axesID[L3GD20_NUMBER_OF_AXES] = {'X', 'Y', 'Z'};

 static uint32_t i;

@@ -136,7 +137,10 @@ int main(void) {
 

     gyroscopeReadCooked(&L3GD20D1, cookeddata);

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

-      chprintf(chp, "COOKED-%c:%f\r\n", axesID[i], cookeddata[i]);

+      chprintf(chp, "COOKED-%c:%.3f\r\n", axesID[i], cookeddata[i]);

+

+    gyroscopeGetTemp(&L3GD20D1, &temperature);

+    chprintf(chp, "TEMP:%.1f C°\r\n", temperature);

 

     chprintf(chp, "Press Button to continue...\r\n");

     chThdSleepMilliseconds(150);

@@ -167,7 +171,10 @@ int main(void) {
 

     gyroscopeReadCooked(&L3GD20D1, cookeddata);

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

-      chprintf(chp, "COOKED-%c:%f\r\n", axesID[i], cookeddata[i]);

+      chprintf(chp, "COOKED-%c:%.3f\r\n", axesID[i], cookeddata[i]);

+

+    gyroscopeGetTemp(&L3GD20D1, &temperature);

+    chprintf(chp, "TEMP:%.1f C°\r\n", temperature);

 

     chThdSleepMilliseconds(150);

 #if CHPRINTF_USE_ANSI_CODE

diff --git a/testhal/STM32/STM32L4xx/SPI-L3GD20/main.c b/testhal/STM32/STM32L4xx/SPI-L3GD20/main.c
index 7bd21d8cb..94850da3b 100644
--- a/testhal/STM32/STM32L4xx/SPI-L3GD20/main.c
+++ b/testhal/STM32/STM32L4xx/SPI-L3GD20/main.c
@@ -30,6 +30,7 @@ static L3GD20Driver L3GD20D1;
 

 static int32_t rawdata[L3GD20_NUMBER_OF_AXES];

 static float cookeddata[L3GD20_NUMBER_OF_AXES];

+static float temperature;

 

 static char axesID[L3GD20_NUMBER_OF_AXES] = {'X', 'Y', 'Z'};

 static uint32_t i;

@@ -109,7 +110,10 @@ int main(void) {
 

     gyroscopeReadCooked(&L3GD20D1, cookeddata);

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

-      chprintf(chp, "COOKED-%c:%f\r\n", axesID[i], cookeddata[i]);

+      chprintf(chp, "COOKED-%c:%.3f\r\n", axesID[i], cookeddata[i]);

+

+    gyroscopeGetTemp(&L3GD20D1, &temperature);

+    chprintf(chp, "TEMP:%.1f C°\r\n", temperature);

 

     chprintf(chp, "Press JOY to continue...\r\n");

     chThdSleepMilliseconds(150);

@@ -140,7 +144,10 @@ int main(void) {
 

     gyroscopeReadCooked(&L3GD20D1, cookeddata);

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

-      chprintf(chp, "COOKED-%c:%f\r\n", axesID[i], cookeddata[i]);

+      chprintf(chp, "COOKED-%c:%.3f\r\n", axesID[i], cookeddata[i]);

+

+    gyroscopeGetTemp(&L3GD20D1, &temperature);

+    chprintf(chp, "TEMP:%.1f C°\r\n", temperature);

 

     chThdSleepMilliseconds(150);

 #if CHPRINTF_USE_ANSI_CODE