L3GD20.cpp

00001 /**
00002  * Copyright (c) 2011 Pololu Corporation.  For more information, see
00003  * 
00004  * http://www.pololu.com/
00005  * http://forum.pololu.com/
00006  * 
00007  * Permission is hereby granted, free of charge, to any person
00008  * obtaining a copy of this software and associated documentation
00009  * files (the "Software"), to deal in the Software without
00010  * restriction, including without limitation the rights to use,
00011  * copy, modify, merge, publish, distribute, sublicense, and/or sell
00012  * copies of the Software, and to permit persons to whom the
00013  * Software is furnished to do so, subject to the following
00014  * conditions:
00015  * 
00016  * The above copyright notice and this permission notice shall be
00017  * included in all copies or substantial portions of the Software.
00018  * 
00019  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
00020  * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
00021  * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
00022  * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
00023  * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
00024  * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
00025  * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
00026  * OTHER DEALINGS IN THE SOFTWARE.
00027  */
00028  
00029  #include "mbed.h"
00030  #include "L3GD20.h"
00031 
00032 
00033 // Defines ////////////////////////////////////////////////////////////////
00034 
00035 // The Arduino two-wire interface uses a 7-bit number for the address, 
00036 // and sets the last bit correctly based on reads and writes
00037 // mbed I2C libraries take the 7-bit address shifted left 1 bit
00038 // #define GYR_ADDRESS (0xD2 >> 1)
00039 #define GYR_ADDRESS 0xD6
00040 
00041 // Public Methods //////////////////////////////////////////////////////////////
00042 
00043 // Constructor
00044 L3GD20::L3GD20(PinName sda, PinName scl)
00045 {
00046     
00047     m_ptr_I2C = new I2C(sda, scl);
00048     
00049     init();
00050 
00051 
00052 }
00053 
00054 L3GD20::L3GD20(I2C* pI2C)
00055 {
00056     m_ptr_I2C = pI2C;
00057     init();
00058 }
00059 
00060 L3GD20::~L3GD20()
00061 {
00062     delete m_ptr_I2C;
00063 }
00064 
00065 void L3GD20::init(void)
00066 {
00067     char reg_v;
00068     
00069     m_ptr_I2C->frequency(200000);
00070     
00071     reg_v = 0;
00072     if(write_reg(GYR_ADDRESS,L3GD20_LOW_ODR,reg_v) == false)
00073     {
00074         debug("Unable to write in regiter \n");
00075     }
00076     
00077   // 0x6F
00078   // DR = 01 (200 Hz ODR); BW = 10 (50 Hz bandwidth); PD = 1 (normal mode); Zen = Yen = Xen = 1 (all axes enabled)
00079     reg_v = 0;    
00080     reg_v |= 0x6F;       
00081     
00082     if(write_reg(GYR_ADDRESS,L3GD20_CTRL_REG1,reg_v) == false)
00083     {
00084         debug("Unable to write in regiter \n");
00085     }
00086 }
00087     
00088     
00089 
00090 
00091 bool L3GD20::read(float *gx, float *gy, float *gz) {
00092     char gyr[6];
00093     bool result = false;
00094     
00095     if(m_ptr_I2C != NULL)
00096     {
00097         if (recv(GYR_ADDRESS, L3GD20_OUT_X_L, gyr, 6)) {
00098         //scale is 8.75 mdps/digit
00099             *gx = float(short(gyr[1] << 8 | gyr[0]))*0.00875;  
00100             *gy =  float(short(gyr[3] << 8 | gyr[2]))*0.00875;
00101             *gz =  float(short(gyr[5] << 8 | gyr[4]))*0.00875;
00102     
00103      
00104             result = true;
00105         }
00106         else
00107         {
00108             debug("Unable to receive \n");
00109         }
00110     }
00111     else
00112     {
00113         debug("Pointer null \n");
00114     }
00115     return result;
00116 }
00117 
00118 
00119 
00120 
00121 bool L3GD20::write_reg(int addr_i2c,int addr_reg, char v)
00122 {
00123     bool result = false;
00124     char data[2] = {addr_reg, v}; 
00125     //__disable_irq();
00126     result = m_ptr_I2C->write(addr_i2c, data, 2) == 0;
00127     //__enable_irq();
00128     return result;
00129 }
00130 
00131 bool L3GD20::read_reg(int addr_i2c,int addr_reg, char *v)
00132 {
00133     char data = addr_reg; 
00134     bool result = false;
00135     
00136     //__disable_irq();
00137     if ((m_ptr_I2C->write(addr_i2c, &data, 1) == 0) && (m_ptr_I2C->read(addr_i2c, &data, 1) == 0)){
00138         *v = data;
00139         result = true;
00140     }
00141     //__enable_irq();
00142     return result;
00143 }
00144 
00145 
00146 bool L3GD20::recv(char sad, char sub, char *buf, int length) {
00147     bool result = false;
00148     
00149     if (length > 1) sub |= 0x80;
00150     //__disable_irq();
00151     result = (m_ptr_I2C->write(sad, &sub, 1, true) == 0);
00152     if(result == false) debug("Unable to Write \n");
00153     result = result && (m_ptr_I2C->read(sad, buf, length) == 0); 
00154     if(result == false) debug("Unable to Read \n");
00155     //__enable_irq();
00156     return result;
00157 }
00158 
00159 bool L3GD20::Convert_to_RadPerSec(float *gx, float *gy, float *gz)
00160 {
00161     bool result = false;
00162     if((gx != NULL) && (gy != NULL) && (gz != NULL) )
00163     {
00164          *gx /= DPS_TO_RPS;
00165          *gy /= DPS_TO_RPS;
00166          *gz /= DPS_TO_RPS;
00167         result = true;
00168     }
00169     return result;
00170 }