Improved BaseGyroscope interface. Updated L3GD20.

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

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.                                                */
 /*===========================================================================*/

os/hal/lib/peripherals/sensors/hal_gyroscope.h

@@ -49,8 +49,19 @@
 #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
+ *          position. Sampled bias will be automatically removed after 
- *          this procedure.
+ *          calling this procedure.
  *
  * @param[in] ip        pointer to a @p BaseGyroscope class.
  * 
@@ -149,7 +160,24 @@ typedef struct {
         (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)			
 /** @} */
 
 /*===========================================================================*/