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