MPU6050 FreeIMU library
Dependents: FreeIMU FreeIMU_external_magnetometer
Fork of MPU6050_tmp by
I2Cdev.cpp
00001 // ported from arduino library: https://github.com/jrowberg/i2cdevlib 00002 // written by szymon gaertig (email: szymon@gaertig.com.pl, website: szymongaertig.pl) 00003 // Changelog: 00004 // 2013-01-08 - first release 00005 00006 #include "MODI2C.h" 00007 #include "I2Cdev.h" 00008 00009 //#define useDebugSerial 00010 00011 I2Cdev::I2Cdev(): debugSerial(USBTX, USBRX), i2c(I2C_SDA,I2C_SCL) 00012 { 00013 00014 } 00015 00016 I2Cdev::I2Cdev(PinName i2cSda, PinName i2cScl): debugSerial(USBTX, USBRX), i2c(i2cSda,i2cScl) 00017 { 00018 00019 } 00020 00021 /** Read a single bit from an 8-bit device register. 00022 * @param devAddr I2C slave device address 00023 * @param regAddr Register regAddr to read from 00024 * @param bitNum Bit position to read (0-7) 00025 * @param data Container for single bit value 00026 * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout) 00027 * @return Status of read operation (true = success) 00028 */ 00029 int8_t I2Cdev::readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data, uint16_t timeout) { 00030 uint8_t b; 00031 uint8_t count = readByte(devAddr, regAddr, &b, timeout); 00032 *data = b & (1 << bitNum); 00033 return count; 00034 } 00035 00036 /** Read a single bit from a 16-bit device register. 00037 * @param devAddr I2C slave device address 00038 * @param regAddr Register regAddr to read from 00039 * @param bitNum Bit position to read (0-15) 00040 * @param data Container for single bit value 00041 * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout) 00042 * @return Status of read operation (true = success) 00043 */ 00044 int8_t I2Cdev::readBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t *data, uint16_t timeout) { 00045 uint16_t b; 00046 uint8_t count = readWord(devAddr, regAddr, &b, timeout); 00047 *data = b & (1 << bitNum); 00048 return count; 00049 } 00050 00051 /** Read multiple bits from an 8-bit device register. 00052 * @param devAddr I2C slave device address 00053 * @param regAddr Register regAddr to read from 00054 * @param bitStart First bit position to read (0-7) 00055 * @param length Number of bits to read (not more than 8) 00056 * @param data Container for right-aligned value (i.e. '101' read from any bitStart position will equal 0x05) 00057 * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout) 00058 * @return Status of read operation (true = success) 00059 */ 00060 int8_t I2Cdev::readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data, uint16_t timeout) { 00061 // 01101001 read byte 00062 // 76543210 bit numbers 00063 // xxx args: bitStart=4, length=3 00064 // 010 masked 00065 // -> 010 shifted 00066 uint8_t count, b; 00067 if ((count = readByte(devAddr, regAddr, &b, timeout)) != 0) { 00068 uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1); 00069 b &= mask; 00070 b >>= (bitStart - length + 1); 00071 *data = b; 00072 } 00073 return count; 00074 } 00075 00076 /** Read multiple bits from a 16-bit device register. 00077 * @param devAddr I2C slave device address 00078 * @param regAddr Register regAddr to read from 00079 * @param bitStart First bit position to read (0-15) 00080 * @param length Number of bits to read (not more than 16) 00081 * @param data Container for right-aligned value (i.e. '101' read from any bitStart position will equal 0x05) 00082 * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout) 00083 * @return Status of read operation (1 = success, 0 = failure, -1 = timeout) 00084 */ 00085 int8_t I2Cdev::readBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t *data, uint16_t timeout) { 00086 // 1101011001101001 read byte 00087 // fedcba9876543210 bit numbers 00088 // xxx args: bitStart=12, length=3 00089 // 010 masked 00090 // -> 010 shifted 00091 uint8_t count; 00092 uint16_t w; 00093 if ((count = readWord(devAddr, regAddr, &w, timeout)) != 0) { 00094 uint16_t mask = ((1 << length) - 1) << (bitStart - length + 1); 00095 w &= mask; 00096 w >>= (bitStart - length + 1); 00097 *data = w; 00098 } 00099 return count; 00100 } 00101 /** Read single byte from an 8-bit device register. 00102 * @param devAddr I2C slave device address 00103 * @param regAddr Register regAddr to read from 00104 * @param data Container for byte value read from device 00105 * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout) 00106 * @return Status of read operation (true = success) 00107 */ 00108 int8_t I2Cdev::readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data, uint16_t timeout) { 00109 return readBytes(devAddr, regAddr, 1, data, timeout); 00110 } 00111 00112 /** Read single word from a 16-bit device register. 00113 * @param devAddr I2C slave device address 00114 * @param regAddr Register regAddr to read from 00115 * @param data Container for word value read from device 00116 * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout) 00117 * @return Status of read operation (true = success) 00118 */ 00119 int8_t I2Cdev::readWord(uint8_t devAddr, uint8_t regAddr, uint16_t *data, uint16_t timeout) { 00120 return readWords(devAddr, regAddr, 1, data, timeout); 00121 } 00122 00123 /** Read multiple bytes from an 8-bit device register. 00124 * @param devAddr I2C slave device address 00125 * @param regAddr First register regAddr to read from 00126 * @param length Number of bytes to read 00127 * @param data Buffer to store read data in 00128 * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout) 00129 * @return Number of bytes read (-1 indicates failure) 00130 */ 00131 int8_t I2Cdev::readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, uint16_t timeout) 00132 { 00133 char command[1]; 00134 command[0] = regAddr; 00135 i2c.write(devAddr, command, 1, true); 00136 i2c.read(devAddr, (char*)data, length); 00137 return length; 00138 } 00139 00140 void I2Cdev::readBytes_nb(uint8_t devAddr, char *command, uint8_t length, uint8_t *data, uint32_t (*function)(uint32_t), void* param) 00141 { 00142 i2c.write(devAddr, command, 1, true); 00143 i2c.read_nb(devAddr, (char*)data, length, function, param); 00144 } 00145 00146 int8_t I2Cdev::readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data, uint16_t timeout) 00147 { 00148 return 0; 00149 } 00150 00151 /** write a single bit in an 8-bit device register. 00152 * @param devAddr I2C slave device address 00153 * @param regAddr Register regAddr to write to 00154 * @param bitNum Bit position to write (0-7) 00155 * @param value New bit value to write 00156 * @return Status of operation (true = success) 00157 */ 00158 bool I2Cdev::writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data) { 00159 uint8_t b; 00160 readByte(devAddr, regAddr, &b); 00161 b = (data != 0) ? (b | (1 << bitNum)) : (b & ~(1 << bitNum)); 00162 return writeByte(devAddr, regAddr, b); 00163 } 00164 00165 /** write a single bit in a 16-bit device register. 00166 * @param devAddr I2C slave device address 00167 * @param regAddr Register regAddr to write to 00168 * @param bitNum Bit position to write (0-15) 00169 * @param value New bit value to write 00170 * @return Status of operation (true = success) 00171 */ 00172 bool I2Cdev::writeBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t data) { 00173 uint16_t w; 00174 readWord(devAddr, regAddr, &w); 00175 w = (data != 0) ? (w | (1 << bitNum)) : (w & ~(1 << bitNum)); 00176 return writeWord(devAddr, regAddr, w); 00177 } 00178 00179 /** Write multiple bits in an 8-bit device register. 00180 * @param devAddr I2C slave device address 00181 * @param regAddr Register regAddr to write to 00182 * @param bitStart First bit position to write (0-7) 00183 * @param length Number of bits to write (not more than 8) 00184 * @param data Right-aligned value to write 00185 * @return Status of operation (true = success) 00186 */ 00187 bool I2Cdev::writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data) { 00188 // 010 value to write 00189 // 76543210 bit numbers 00190 // xxx args: bitStart=4, length=3 00191 // 00011100 mask byte 00192 // 10101111 original value (sample) 00193 // 10100011 original & ~mask 00194 // 10101011 masked | value 00195 uint8_t b; 00196 if (readByte(devAddr, regAddr, &b) != 0) { 00197 uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1); 00198 data <<= (bitStart - length + 1); // shift data into correct position 00199 data &= mask; // zero all non-important bits in data 00200 b &= ~(mask); // zero all important bits in existing byte 00201 b |= data; // combine data with existing byte 00202 return writeByte(devAddr, regAddr, b); 00203 } else { 00204 return false; 00205 } 00206 } 00207 00208 /** Write multiple bits in a 16-bit device register. 00209 * @param devAddr I2C slave device address 00210 * @param regAddr Register regAddr to write to 00211 * @param bitStart First bit position to write (0-15) 00212 * @param length Number of bits to write (not more than 16) 00213 * @param data Right-aligned value to write 00214 * @return Status of operation (true = success) 00215 */ 00216 bool I2Cdev::writeBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t data) { 00217 // 010 value to write 00218 // fedcba9876543210 bit numbers 00219 // xxx args: bitStart=12, length=3 00220 // 0001110000000000 mask byte 00221 // 1010111110010110 original value (sample) 00222 // 1010001110010110 original & ~mask 00223 // 1010101110010110 masked | value 00224 uint16_t w; 00225 if (readWord(devAddr, regAddr, &w) != 0) { 00226 uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1); 00227 data <<= (bitStart - length + 1); // shift data into correct position 00228 data &= mask; // zero all non-important bits in data 00229 w &= ~(mask); // zero all important bits in existing word 00230 w |= data; // combine data with existing word 00231 return writeWord(devAddr, regAddr, w); 00232 } else { 00233 return false; 00234 } 00235 } 00236 00237 /** Write single byte to an 8-bit device register. 00238 * @param devAddr I2C slave device address 00239 * @param regAddr Register address to write to 00240 * @param data New byte value to write 00241 * @return Status of operation (true = success) 00242 */ 00243 bool I2Cdev::writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data) { 00244 return writeBytes(devAddr, regAddr, 1, &data); 00245 } 00246 00247 /** Write single word to a 16-bit device register. 00248 * @param devAddr I2C slave device address 00249 * @param regAddr Register address to write to 00250 * @param data New word value to write 00251 * @return Status of operation (true = success) 00252 */ 00253 bool I2Cdev::writeWord(uint8_t devAddr, uint8_t regAddr, uint16_t data) { 00254 return writeWords(devAddr, regAddr, 1, &data); 00255 } 00256 00257 uint32_t writefinhdl(uint32_t param){ 00258 //free((void*)param); 00259 ((I2Cdev*)param)->freebuffer(); 00260 return 0; 00261 } 00262 00263 bool I2Cdev::writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data) 00264 { 00265 char* mem = NULL; 00266 mem = allocbuffer(); 00267 *mem = regAddr; 00268 memcpy(mem+1, data, length); 00269 00270 i2c.write(devAddr, mem, length+1, &writefinhdl, this); 00271 return true; 00272 } 00273 00274 char *I2Cdev::allocbuffer(){ 00275 char *buff = (char *)pool.alloc(); 00276 while (buff==NULL) buff = (char *)pool.alloc(); 00277 queue.put(buff); 00278 return buff; 00279 } 00280 00281 void I2Cdev::freebuffer(){ 00282 osEvent evt = queue.get(0); 00283 pool.free((char(*)[14])evt.value.p); 00284 } 00285 00286 bool I2Cdev::writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data) 00287 { 00288 return true; 00289 } 00290 00291 uint16_t I2Cdev::readTimeout(void) 00292 { 00293 return 0; 00294 }
Generated on Sat Jul 16 2022 02:18:11 by 1.7.2