Improved BaseGyroscope interface. Updated L3GD20.

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

2 files changed, 161 insertions, 10 deletions

diff --git a/os/ex/ST/l3gd20.c b/os/ex/ST/l3gd20.c
index 63b902058..2fa329a76 100644
--- a/os/ex/ST/l3gd20.c
+++ b/os/ex/ST/l3gd20.c
@@ -257,7 +257,7 @@ static msg_t sample_bias(void *ip) {
   osalDbgCheck(ip != NULL);
 
   osalDbgAssert((((L3GD20Driver *)ip)->state == L3GD20_READY),
-              "calibrate(), invalid state");
+                "sample_bias(), invalid state");
 
   for(i = 0; i < L3GD20_BIAS_ACQ_TIMES; i++){
     read_raw(ip, raw);
@@ -273,6 +273,21 @@ static msg_t sample_bias(void *ip) {
   return MSG_OK;
 }
 
+static msg_t set_bias(void *ip, int32_t *bp) {
+  uint32_t i;
+  
+  osalDbgCheck((ip != NULL) && (bp !=NULL));
+
+  osalDbgAssert((((L3GD20Driver *)ip)->state == L3GD20_READY) ||
+                (((L3GD20Driver *)ip)->state == L3GD20_STOP),
+                "set_bias(), invalid state");
+  
+  for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++) {
+    ((L3GD20Driver *)ip)->bias[i] = bp[i];
+  }
+  return MSG_OK;
+}
+
 static msg_t reset_bias(void *ip) {
   uint32_t i;
 
@@ -287,16 +302,64 @@ static msg_t reset_bias(void *ip) {
   return MSG_OK;
 }
 
+static msg_t set_sensivity(void *ip, float *sp) {
+  uint32_t i;
+  
+  osalDbgCheck((ip != NULL) && (sp !=NULL));
+
+  osalDbgAssert((((L3GD20Driver *)ip)->state == L3GD20_READY),
+                "set_sensivity(), invalid state");
+  
+  for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++) {
+    ((L3GD20Driver *)ip)->sensitivity[i] = sp[i];
+  }
+  return MSG_OK;
+}
+
+static msg_t reset_sensivity(void *ip) {
+  uint32_t i;
+
+  osalDbgCheck(ip != NULL);
+
+  osalDbgAssert((((L3GD20Driver *)ip)->state == L3GD20_READY),
+                "reset_sensivity(), invalid state");
+
+  if(((L3GD20Driver *)ip)->config->fullscale == L3GD20_FS_250DPS)
+    for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++)
+      ((L3GD20Driver *)ip)->sensitivity[i] = L3GD20_SENS_250DPS;
+  else if(((L3GD20Driver *)ip)->config->fullscale == L3GD20_FS_500DPS)
+	for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++)
+      ((L3GD20Driver *)ip)->sensitivity[i] = L3GD20_SENS_500DPS;
+  else if(((L3GD20Driver *)ip)->config->fullscale == L3GD20_FS_2000DPS)
+	for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++)
+      ((L3GD20Driver *)ip)->sensitivity[i] = L3GD20_SENS_2000DPS;
+  return MSG_OK;
+}
+
+static msg_t enable_temperature_compensation(void *ip) {
+  (void) ip;
+  /* TODO complete this function */
+  return 0;
+}
+
+static msg_t disable_temperature_compensation(void *ip) {
+  (void) ip;
+  /* TODO complete this function */
+  return 0;
+}
+
 static const struct BaseSensorVMT vmt_basesensor = {
   get_axes_number, read_raw, read_cooked
 };
 
 static const struct BaseGyroscopeVMT vmt_basegyroscope = {
   get_axes_number, read_raw, read_cooked,
-  sample_bias, reset_bias
+  sample_bias, set_bias, reset_bias,
+  set_sensivity, reset_sensivity,
+  enable_temperature_compensation,
+  disable_temperature_compensation
 };
 
-
 /*===========================================================================*/
 /* Driver exported functions.                                                */
 /*===========================================================================*/
diff --git a/os/hal/lib/peripherals/sensors/hal_gyroscope.h b/os/hal/lib/peripherals/sensors/hal_gyroscope.h
index e61d06b0f..5a2db78b6 100644
--- a/os/hal/lib/peripherals/sensors/hal_gyroscope.h
+++ b/os/hal/lib/peripherals/sensors/hal_gyroscope.h
@@ -49,9 +49,20 @@
 #define _base_gyroscope_methods_alone                                       \

   /* Invoke the sample bias procedure.*/                                    \

   msg_t (*sample_bias)(void *instance);                                     \

+  /* Invoke the set bias procedure.*/                                       \

+  msg_t (*set_bias)(void *instance, int32_t biases[]);                      \

   /* Remove bias stored data.*/                                             \

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

-

+  msg_t (*reset_bias)(void *instance);                                      \

+  /* 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);                                 

+  

+  

 /**

  * @brief   BaseGyroscope specific methods with inherited ones.

  */

@@ -134,8 +145,8 @@ typedef struct {
 /**

  * @brief   Gyroscope bias sampling procedure.

  * @note    During this procedure gyroscope must be kept hold in the rest

- *          position. Sampled bias will be automatically removed after calling

- *          this procedure.

+ *          position. Sampled bias will be automatically removed after 

+ *          calling this procedure.

  *

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

  * 

@@ -145,11 +156,28 @@ typedef struct {
  *

  * @api

  */

-#define gyroscopeSampleBias(ip)                                              \

+#define gyroscopeSampleBias(ip)                                             \

         (ip)->vmt_basegyroscope->sample_bias(ip)

 

 /**

- * @brief   Reset bias data restoring it to zero.

+ * @brief   Updates gyroscope bias data from received buffer.

+ * @note    The bias buffer must have the same length of the

+ *          the gyroscope axes number.

+ *

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

+ * @param[in] bp        pointer to a buffer of bias values.

+ * 

+ * @return              The operation status.

+ * @retval MSG_OK       if the function succeeded.

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

+ *

+ * @api

+ */

+#define gyroscopeSetBias(ip, bp)                                            \

+        (ip)->vmt_basegyroscope->set_bias(ip, bp)

+		

+/**

+ * @brief   Reset gyroscope bias data restoring it to zero.

  *

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

  * 

@@ -159,8 +187,68 @@ typedef struct {
  *

  * @api

  */

-#define gyroscopeResetCalibration(ip)                                       \

+#define gyroscopeResetBias(ip)                                               \

         (ip)->vmt_basegyroscope->reset_bias(ip)

+		

+/**

+ * @brief   Updates gyroscope sensitivity data from received buffer.

+ * @note    The sensitivity buffer must have the same length of the

+ *          the gyroscope axes number.

+ *

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

+ * @param[in] sp        pointer to a buffer of sensitivity values.

+ * 

+ * @return              The operation status.

+ * @retval MSG_OK       if the function succeeded.

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

+ *

+ * @api

+ */

+#define gyroscopeSetSensitivity(ip, sp)                                     \

+        (ip)->vmt_basegyroscope->set_sensitivity(ip, sp)

+		

+/**

+ * @brief   Reset gyroscope sensitivity data restoring it to its typical 

+ *          value.

+ *

+ * @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 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)			

 /** @} */

 

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