cvbxcvxcv
Embed:
(wiki syntax)
Show/hide line numbers
Lis2dh12.cpp
00001 /* 00002 * 00003 * LIS2DH12 MEMS digital output motion sensor 00004 * More info @ aconno.de 00005 * Made by Jurica Resetar 00006 * jurica_resetar@yahoo.com 00007 * 00008 */ 00009 00010 #include "Lis2dh12.h" 00011 #include "Lis2dh12_regs.h" 00012 00013 Lis2dh12::Lis2dh12(I2C *i2c_, char address) : i2c(i2c_, address), 00014 spiInterface(false){ 00015 } 00016 00017 Lis2dh12::Lis2dh12(SPI *_spi, DigitalOut *_cs) : i2c(NULL, 0), 00018 spi(_spi), 00019 cs(_cs), 00020 spiInterface(true){ 00021 *cs = 1; 00022 spi->format(8, 3); 00023 spi->frequency(1000000); 00024 } 00025 00026 uint8_t Lis2dh12::whoIAm(){ 00027 char regAddr = (char)WHO_AM_I; 00028 char regData; 00029 readFromReg(regAddr, (uint8_t*)®Data, 1); 00030 return (uint8_t)regData; 00031 } 00032 00033 uint8_t Lis2dh12::setMode(Mode mode){ 00034 char ctrl1Copy; 00035 char ctrl4Copy; 00036 uint8_t success; 00037 00038 readFromReg((char)CTRL_REG1, (uint8_t*)&ctrl1Copy, 1); 00039 readFromReg((char)CTRL_REG4, (uint8_t*)&ctrl4Copy, 1); 00040 00041 switch(mode){ 00042 case HIGH_RES: 00043 ctrl1Copy &= 0xF7; 00044 ctrl4Copy |= 0x08; 00045 break; 00046 case NORMAL: 00047 ctrl1Copy &= 0xF7; 00048 ctrl4Copy &= 0xF7; 00049 break; 00050 case LOW_POWER: 00051 ctrl1Copy |= 0x08; 00052 ctrl4Copy &= 0xF7; 00053 break; 00054 default: 00055 return 0; 00056 } 00057 writeToReg((char)CTRL_REG1, (uint8_t*)&ctrl1Copy, 1); 00058 success = writeToReg((char)CTRL_REG4, (uint8_t*)&ctrl4Copy, 1); 00059 return success; 00060 } 00061 00062 uint8_t Lis2dh12::enableAxes(Axis axis){ 00063 char ctrl1Copy; 00064 readFromReg((char)CTRL_REG1, (uint8_t*)&ctrl1Copy, 1); 00065 ctrl1Copy |= axis; 00066 writeToReg((char)CTRL_REG1, (uint8_t*)&ctrl1Copy, 1); 00067 return 0; 00068 } 00069 00070 uint8_t Lis2dh12::disableAxes(Axis axis){ 00071 char ctrl1Copy; 00072 readFromReg((char)CTRL_REG1, (uint8_t*)&ctrl1Copy, 1); 00073 ctrl1Copy &= ~(1 << axis); 00074 writeToReg((char)CTRL_REG1, (uint8_t*)&ctrl1Copy, 1); 00075 return 0; 00076 } 00077 00078 int16_t Lis2dh12::readXAxis(){ 00079 int16_t rawData; 00080 char tempData; 00081 /* 00082 // Make sure new data is ready 00083 do{ 00084 readFromReg((char)STATUS, (uint8_t*)&tempData, 1); 00085 }while(!(tempData & 0x08)); 00086 do{ 00087 readFromReg((char)STATUS, (uint8_t*)&tempData, 1); 00088 }while(!(tempData & 0x80)); 00089 */ 00090 00091 // Same data have been overwritten 00092 readFromReg((char)OUT_X_H, (uint8_t*)&tempData, 1); 00093 rawData = (int8_t)tempData << 8; 00094 // readFromReg((char)OUT_X_L, (uint8_t*)&rawData, 1); 00095 // readFromReg((char)OUT_X_H, ((uint8_t*)&rawData)+1, 1); 00096 00097 if (rawData >= 0) 00098 rawData = (rawData>>4); 00099 else 00100 rawData = (rawData>>4) | 0xF000; 00101 00102 return rawData; 00103 } 00104 00105 int16_t Lis2dh12::readYAxis(){ 00106 int16_t rawData; 00107 char tempData; 00108 readFromReg((char)OUT_Y_H, (uint8_t*)&tempData, 1); 00109 rawData = (int8_t)tempData << 8; 00110 // readFromReg((char)OUT_Y_L, (uint8_t*)&rawData, 1); 00111 // readFromReg((char)OUT_Y_H, ((uint8_t*)&rawData)+1, 1); 00112 00113 if (rawData >= 0) 00114 rawData = (rawData>>4); 00115 else 00116 rawData = (rawData>>4) | 0xF000; 00117 00118 return rawData; 00119 } 00120 00121 int16_t Lis2dh12::readZAxis(){ 00122 int16_t rawData = 0; 00123 char tempData; 00124 readFromReg((char)OUT_Z_H, (uint8_t*)&tempData, 1); 00125 rawData = (int8_t)tempData << 8; 00126 // readFromReg((char)OUT_Z_L, (uint8_t*)&rawData, 1); 00127 // readFromReg((char)OUT_Z_H, ((uint8_t*)&rawData)+1, 1); 00128 00129 if (rawData >= 0) 00130 rawData = (rawData>>4); 00131 else 00132 rawData = (rawData>>4) | 0xF000; 00133 00134 return rawData; 00135 } 00136 00137 uint8_t Lis2dh12::setODR(Odr odr){ 00138 char ctrl1Copy; 00139 readFromReg((char)CTRL_REG1, (uint8_t*)&ctrl1Copy, 1); 00140 ctrl1Copy |= (odr << 4); 00141 writeToReg((char)CTRL_REG1, (uint8_t*)&ctrl1Copy, 1); 00142 return 0; 00143 } 00144 00145 uint8_t Lis2dh12::setScale(Scale scale){ 00146 char ctrl4Copy; 00147 readFromReg((char)CTRL_REG4, (uint8_t*)&ctrl4Copy, 1); 00148 ctrl4Copy |= (scale << 4); 00149 writeToReg((char)CTRL_REG4, (uint8_t*)&ctrl4Copy, 1); 00150 return 0; 00151 } 00152 00153 /* Interrupt activity 1 driven to INT1 pad */ 00154 // TODO: Napraviti tako da postoji samo jedna metoda int1Setup koja prima gro 00155 // parametara: THS, DUR, Latch... 00156 uint8_t Lis2dh12::int1Setup(uint8_t setup){ 00157 char data = setup; 00158 writeToReg((char)CTRL_REG3, (uint8_t*)&data, 1); 00159 return 0; 00160 } 00161 00162 uint8_t Lis2dh12::int1Latch(uint8_t enable){ 00163 char ctrl5Copy; 00164 readFromReg((char)CTRL_REG5, (uint8_t*)&ctrl5Copy, 1); 00165 ctrl5Copy |= enable; 00166 writeToReg((char)CTRL_REG5, (uint8_t*)&ctrl5Copy, 1); 00167 return 0; 00168 } 00169 00170 uint8_t Lis2dh12::int1Duration(uint8_t duration){ 00171 char copy = duration; 00172 writeToReg((char)INT1_DURATION, (uint8_t*)©, 1); 00173 return 0; 00174 } 00175 00176 uint8_t Lis2dh12::int1Threshold(uint8_t threshold){ 00177 char copy = threshold; 00178 writeToReg((char)INT1_THS, (uint8_t*)©, 1); 00179 return 0; 00180 } 00181 00182 uint8_t Lis2dh12::int1Config(uint8_t config){ 00183 char copy = config; 00184 writeToReg((char)INT1_CFG, (uint8_t*)©, 1); 00185 return 0; 00186 } 00187 00188 void Lis2dh12::clearIntFlag(){ 00189 char data; 00190 readFromReg((char)INT1_SRC, (uint8_t*)&data, 1); 00191 } 00192 00193 uint8_t Lis2dh12::readFromReg(uint8_t regAddr, uint8_t *buff, size_t buffSize) 00194 { 00195 // Most significant bit represents read from register, bit after it 00196 // represents address increment if multiple read, which is enabled. 00197 const uint8_t spiSetup = 0xC0; 00198 00199 uint8_t retVal = 0; 00200 00201 if( spiInterface ) 00202 { 00203 *cs = 0; 00204 00205 spi->write(spiSetup | regAddr); 00206 00207 while(buffSize--) 00208 { 00209 *buff = spi->write(0x00); 00210 buff++; 00211 } 00212 00213 *cs = 1; 00214 } 00215 else 00216 { 00217 retVal = i2c.readFromReg((char)regAddr, (char*)buff, buffSize); 00218 } 00219 00220 return retVal; 00221 } 00222 00223 uint8_t Lis2dh12::writeToReg(uint8_t regAddr, uint8_t *buff, size_t buffSize) 00224 { 00225 // Most significant bit represents write from register, bit after it 00226 // represents address increment if multiple write, which is enabled. 00227 const uint8_t spiSetup = 0x40; 00228 00229 uint8_t retVal = 0; 00230 00231 if( spiInterface ) 00232 { 00233 *cs = 0; 00234 00235 spi->write(spiSetup | regAddr); 00236 00237 while(buffSize--) 00238 { 00239 spi->write(*buff); 00240 buff++; 00241 } 00242 00243 *cs = 1; 00244 } 00245 else 00246 { 00247 retVal = i2c.writeToReg((char)regAddr, (char*)buff, buffSize); 00248 } 00249 00250 return retVal; 00251 }
Generated on Tue Jul 12 2022 17:06:34 by
1.7.2