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