My attempt to make a better lib (in development)
Fork of L3GD20 by
Embed:
(wiki syntax)
Show/hide line numbers
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 }
Generated on Wed Jul 20 2022 12:14:40 by
1.7.2
Alexandre Salconi-Denis
