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

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) {
+static msg_t get_temperature(void *ip, float* tempp) {
-  (void) ip;
-  /* TODO complete this function */
-  return 0;
-}
 
-static msg_t disable_temperature_compensation(void *ip) {
+  *tempp = (int8_t)l3gd20SPIReadRegister(((L3GD20Driver *)ip)->config->spip,
-  (void) ip;
+                                       L3GD20_AD_OUT_TEMP);
-  /* 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,
+  set_sensivity, reset_sensivity
-  enable_temperature_compensation,
+};
-  disable_temperature_compensation
+
+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++)

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.*/
+  /** @brief L3GD20 Virtual Methods Table.
-  const struct L3GD20VMT *vmt;
+   * @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.                                                    */
 /*===========================================================================*/

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);                               \
+  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);                                 
   
   
 /**
@@ -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)			
 /** @} */
 
 /*===========================================================================*/

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

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