Diff: L3GD20.cpp

Revision:

3:17c3c3f59c4d

Parent:

2:b45dbca259f8

--- a/L3GD20.cpp	Mon May 22 15:18:15 2017 +0000
+++ b/L3GD20.cpp	Mon Aug 07 01:25:38 2017 +0000
@@ -38,45 +38,81 @@
 // #define GYR_ADDRESS (0xD2 >> 1)
 #define GYR_ADDRESS 0xD6
 
-
 // Public Methods //////////////////////////////////////////////////////////////
 
 // Constructor
-L3GD20::L3GD20(PinName sda, PinName scl):
-    _L3GD20(sda, scl)
+L3GD20::L3GD20(PinName sda, PinName scl)
+{
+    
+    m_ptr_I2C = new I2C(sda, scl);
+    
+    init();
+
+
+}
+
+L3GD20::L3GD20(I2C* pI2C)
+{
+    m_ptr_I2C = pI2C;
+    init();
+}
+
+L3GD20::~L3GD20()
+{
+    delete m_ptr_I2C;
+}
+
+void L3GD20::init(void)
 {
     char reg_v;
-    _L3GD20.frequency(200000);
+    
+    m_ptr_I2C->frequency(200000);
     
-    reg_v = 0;          
-    write_reg(GYR_ADDRESS,L3GD20_LOW_ODR,reg_v);
+    reg_v = 0;
+    if(write_reg(GYR_ADDRESS,L3GD20_LOW_ODR,reg_v) == false)
+    {
+        debug("Unable to write in regiter \n");
+    }
     
   // 0x6F
   // DR = 01 (200 Hz ODR); BW = 10 (50 Hz bandwidth); PD = 1 (normal mode); Zen = Yen = Xen = 1 (all axes enabled)
     reg_v = 0;    
     reg_v |= 0x6F;       
-    write_reg(GYR_ADDRESS,L3GD20_CTRL_REG1,reg_v);
     
-
-
+    if(write_reg(GYR_ADDRESS,L3GD20_CTRL_REG1,reg_v) == false)
+    {
+        debug("Unable to write in regiter \n");
+    }
 }
-
+    
+    
 
 
 bool L3GD20::read(float *gx, float *gy, float *gz) {
     char gyr[6];
- 
-    if (recv(GYR_ADDRESS, L3GD20_OUT_X_L, gyr, 6)) {
-    //scale is 8.75 mdps/digit
-        *gx = float(short(gyr[1] << 8 | gyr[0]))*0.00875;  
-        *gy =  float(short(gyr[3] << 8 | gyr[2]))*0.00875;
-        *gz =  float(short(gyr[5] << 8 | gyr[4]))*0.00875;
-
- 
-        return true;
+    bool result = false;
+    
+    if(m_ptr_I2C != NULL)
+    {
+        if (recv(GYR_ADDRESS, L3GD20_OUT_X_L, gyr, 6)) {
+        //scale is 8.75 mdps/digit
+            *gx = float(short(gyr[1] << 8 | gyr[0]))*0.00875;  
+            *gy =  float(short(gyr[3] << 8 | gyr[2]))*0.00875;
+            *gz =  float(short(gyr[5] << 8 | gyr[4]))*0.00875;
+    
+     
+            result = true;
+        }
+        else
+        {
+            debug("Unable to receive \n");
+        }
     }
- 
-    return false;
+    else
+    {
+        debug("Pointer null \n");
+    }
+    return result;
 }
 
 
@@ -84,8 +120,12 @@
 
 bool L3GD20::write_reg(int addr_i2c,int addr_reg, char v)
 {
+    bool result = false;
     char data[2] = {addr_reg, v}; 
-    return L3GD20::_L3GD20.write(addr_i2c, data, 2) == 0;
+    //__disable_irq();
+    result = m_ptr_I2C->write(addr_i2c, data, 2) == 0;
+    //__enable_irq();
+    return result;
 }
 
 bool L3GD20::read_reg(int addr_i2c,int addr_reg, char *v)
@@ -93,18 +133,38 @@
     char data = addr_reg; 
     bool result = false;
     
-    __disable_irq();
-    if ((_L3GD20.write(addr_i2c, &data, 1) == 0) && (_L3GD20.read(addr_i2c, &data, 1) == 0)){
+    //__disable_irq();
+    if ((m_ptr_I2C->write(addr_i2c, &data, 1) == 0) && (m_ptr_I2C->read(addr_i2c, &data, 1) == 0)){
         *v = data;
         result = true;
     }
-    __enable_irq();
+    //__enable_irq();
     return result;
 }
 
 
 bool L3GD20::recv(char sad, char sub, char *buf, int length) {
+    bool result = false;
+    
     if (length > 1) sub |= 0x80;
- 
-    return _L3GD20.write(sad, &sub, 1, true) == 0 && _L3GD20.read(sad, buf, length) == 0;
+    //__disable_irq();
+    result = (m_ptr_I2C->write(sad, &sub, 1, true) == 0);
+    if(result == false) debug("Unable to Write \n");
+    result = result && (m_ptr_I2C->read(sad, buf, length) == 0); 
+    if(result == false) debug("Unable to Read \n");
+    //__enable_irq();
+    return result;
+}
+
+bool L3GD20::Convert_to_RadPerSec(float *gx, float *gy, float *gz)
+{
+    bool result = false;
+    if((gx != NULL) && (gy != NULL) && (gz != NULL) )
+    {
+         *gx /= DPS_TO_RPS;
+         *gy /= DPS_TO_RPS;
+         *gz /= DPS_TO_RPS;
+        result = true;
+    }
+    return result;
 }
\ No newline at end of file