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

2 files changed, 38 insertions, 18 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.                                                    */
 /*===========================================================================*/