From f2f3035c3c49daa3a62aa7606455da90d350190e Mon Sep 17 00:00:00 2001
From: Rocco Marco Guglielmi <roccomarco.guglielmi@gmail.com>
Date: Sat, 10 Mar 2018 13:53:44 +0000
Subject: Improved EX drivers

git-svn-id: svn://svn.code.sf.net/p/chibios/svn/trunk@11685 110e8d01-0319-4d1e-a829-52ad28d1bb01
---
 os/ex/ST/l3gd20.c | 26 ++++++++++++++------------
 1 file changed, 14 insertions(+), 12 deletions(-)

(limited to 'os/ex/ST/l3gd20.c')

diff --git a/os/ex/ST/l3gd20.c b/os/ex/ST/l3gd20.c
index fcb6dd17d..4313341e6 100644
--- a/os/ex/ST/l3gd20.c
+++ b/os/ex/ST/l3gd20.c
@@ -286,7 +286,7 @@ static msg_t gyro_reset_bias(void *ip) {
                 "gyro_reset_bias(), invalid state");
 
   for(i = 0; i < L3GD20_GYRO_NUMBER_OF_AXES; i++)
-    devp->gyrobias[i] = 0.0;
+    devp->gyrobias[i] = L3GD20_GYRO_BIAS;
   return MSG_OK;
 }
 
@@ -345,13 +345,13 @@ static msg_t gyro_reset_sensivity(void *ip) {
 
   if(devp->config->gyrofullscale == L3GD20_FS_250DPS)
     for(i = 0; i < L3GD20_GYRO_NUMBER_OF_AXES; i++)
-      devp->gyrosensitivity[i] = L3GD20_SENS_250DPS;
+      devp->gyrosensitivity[i] = L3GD20_GYRO_SENS_250DPS;
   else if(devp->config->gyrofullscale == L3GD20_FS_500DPS)
 	for(i = 0; i < L3GD20_GYRO_NUMBER_OF_AXES; i++)
-      devp->gyrosensitivity[i] = L3GD20_SENS_500DPS;
+      devp->gyrosensitivity[i] = L3GD20_GYRO_SENS_500DPS;
   else if(devp->config->gyrofullscale == L3GD20_FS_2000DPS)
 	for(i = 0; i < L3GD20_GYRO_NUMBER_OF_AXES; i++)
-      devp->gyrosensitivity[i] = L3GD20_SENS_2000DPS;
+      devp->gyrosensitivity[i] = L3GD20_GYRO_SENS_2000DPS;
   else {
     osalDbgAssert(FALSE, "gyro_reset_sensivity(), full scale issue");
     return MSG_RESET;
@@ -468,14 +468,11 @@ static const struct BaseGyroscopeVMT vmt_gyroscope = {
  * @init
  */
 void l3gd20ObjectInit(L3GD20Driver *devp) {
-  uint32_t i;
-  
   devp->vmt = &vmt_device;
   devp->gyro_if.vmt = &vmt_gyroscope;
   
   devp->config = NULL;
-  for(i = 0; i < L3GD20_GYRO_NUMBER_OF_AXES; i++)
-    devp->gyrobias[i] = 0.0f;
+
   devp->state  = L3GD20_STOP;
 }
 
@@ -560,7 +557,7 @@ void l3gd20Start(L3GD20Driver *devp, const L3GD20Config *config) {
     devp->gyrofullscale = L3GD20_250DPS;
     for(i = 0; i < L3GD20_GYRO_NUMBER_OF_AXES; i++) {
       if (devp->config->gyrosensitivity == NULL)
-        devp->gyrosensitivity[i] = L3GD20_SENS_250DPS;
+        devp->gyrosensitivity[i] = L3GD20_GYRO_SENS_250DPS;
       else
         devp->gyrosensitivity[i] = devp->config->gyrosensitivity[i];
     }
@@ -569,7 +566,7 @@ void l3gd20Start(L3GD20Driver *devp, const L3GD20Config *config) {
     devp->gyrofullscale = L3GD20_500DPS;
     for(i = 0; i < L3GD20_GYRO_NUMBER_OF_AXES; i++) {
       if (devp->config->gyrosensitivity == NULL)
-        devp->gyrosensitivity[i] = L3GD20_SENS_500DPS;
+        devp->gyrosensitivity[i] = L3GD20_GYRO_SENS_500DPS;
       else
         devp->gyrosensitivity[i] = devp->config->gyrosensitivity[i];
     }
@@ -578,19 +575,24 @@ void l3gd20Start(L3GD20Driver *devp, const L3GD20Config *config) {
     devp->gyrofullscale = L3GD20_2000DPS;
     for(i = 0; i < L3GD20_GYRO_NUMBER_OF_AXES; i++) {
       if (devp->config->gyrosensitivity == NULL)
-        devp->gyrosensitivity[i] = L3GD20_SENS_2000DPS;
+        devp->gyrosensitivity[i] = L3GD20_GYRO_SENS_2000DPS;
       else
         devp->gyrosensitivity[i] = devp->config->gyrosensitivity[i];
     }
   }
   else
     osalDbgAssert(FALSE, "l3gd20Start(), full scale issue");
-  
+
+  /* Storing bias information.*/  
   if(devp->config->gyrobias != NULL) {
     for(i = 0; i < L3GD20_GYRO_NUMBER_OF_AXES; i++) {
       devp->gyrobias[i] = devp->config->gyrobias[i];
     }
   }
+  else {
+    for(i = 0; i < L3GD20_GYRO_NUMBER_OF_AXES; i++)
+      devp->gyrobias[i] = L3GD20_GYRO_BIAS;
+  }
   
   /* This is the Gyroscope transient recovery time.*/
   osalThreadSleepMilliseconds(10);
-- 
cgit v1.2.3