Akash Vibhute / MPU6050_DMP_Nucleo-I2Cdev

MPU DMP code modified for use with ST Nucelo boards. This Library makes use of I2Cdev.

Dependents:   MPU9150_nucleo_i2cdev Orion_newPCB_test_LV Orion_PCB_test_Faulhaber_gr41_wptcmd_V1 MPU9150_nucleo_i2cdev ... more

Fork of MPU6050-DMP-Ian by Ian Hua

I2C/I2Cdev.cpp@17:d12e7a7d4d02, 2016-05-19 (annotated)

Committer:
akashvibhute
Date:
Thu May 19 06:38:09 2016 +0000
Revision:
17:d12e7a7d4d02
Parent:
16:babac099274f 
added getMag function to query onboard magnetometer

Who changed what in which revision?

UserRevisionLine numberNew contents of line
akashvibhute 15:d4284f5b4fc7 1 /*
akashvibhute 15:d4284f5b4fc7 2 *
akashvibhute 15:d4284f5b4fc7 3 * Modified by Akash Vibhute on 29 March 2015
akashvibhute 15:d4284f5b4fc7 4 * This library now works well with Nucelo boards from ST
akashvibhute 15:d4284f5b4fc7 5 *
akashvibhute 15:d4284f5b4fc7 6 */
akashvibhute 15:d4284f5b4fc7 7
akashvibhute 15:d4284f5b4fc7 8
pHysiX 6:2dc23167c8d8 9 // ported from arduino library: https://github.com/jrowberg/i2cdevlib
pHysiX 6:2dc23167c8d8 10 // written by szymon gaertig (email: szymon@gaertig.com.pl, website: szymongaertig.pl)
pHysiX 6:2dc23167c8d8 11 // Changelog:
pHysiX 6:2dc23167c8d8 12 // 2013-01-08 - first release
pHysiX 6:2dc23167c8d8 13
pHysiX 6:2dc23167c8d8 14 #include "I2Cdev.h"
pHysiX 6:2dc23167c8d8 15
pHysiX 6:2dc23167c8d8 16 #define useDebugSerial
pHysiX 6:2dc23167c8d8 17
pHysiX 6:2dc23167c8d8 18 I2Cdev::I2Cdev(): i2c(I2C_SDA,I2C_SCL), debugSerial(USBTX, USBRX)
pHysiX 6:2dc23167c8d8 19 {
pHysiX 6:2dc23167c8d8 20 i2c.frequency(400000);
pHysiX 6:2dc23167c8d8 21 debugSerial.baud(115200);
pHysiX 6:2dc23167c8d8 22 }
pHysiX 6:2dc23167c8d8 23
pHysiX 6:2dc23167c8d8 24 I2Cdev::I2Cdev(PinName i2cSda, PinName i2cScl): i2c(i2cSda,i2cScl), debugSerial(USBTX, USBRX)
pHysiX 6:2dc23167c8d8 25 {
pHysiX 6:2dc23167c8d8 26 i2c.frequency(400000);
pHysiX 6:2dc23167c8d8 27 debugSerial.baud(115200);
pHysiX 6:2dc23167c8d8 28 }
pHysiX 6:2dc23167c8d8 29
pHysiX 6:2dc23167c8d8 30 /** Read a single bit from an 8-bit device register.
pHysiX 6:2dc23167c8d8 31 * @param devAddr I2C slave device address
pHysiX 6:2dc23167c8d8 32 * @param regAddr Register regAddr to read from
pHysiX 6:2dc23167c8d8 33 * @param bitNum Bit position to read (0-7)
pHysiX 6:2dc23167c8d8 34 * @param data Container for single bit value
pHysiX 6:2dc23167c8d8 35 * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
pHysiX 6:2dc23167c8d8 36 * @return Status of read operation (true = success)
pHysiX 6:2dc23167c8d8 37 */
pHysiX 6:2dc23167c8d8 38 int8_t I2Cdev::readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data, uint16_t timeout)
pHysiX 6:2dc23167c8d8 39 {
pHysiX 6:2dc23167c8d8 40 uint8_t b;
pHysiX 6:2dc23167c8d8 41 uint8_t count = readByte(devAddr, regAddr, &b, timeout);
pHysiX 6:2dc23167c8d8 42 *data = b & (1 << bitNum);
pHysiX 6:2dc23167c8d8 43 return count;
pHysiX 6:2dc23167c8d8 44 }
pHysiX 6:2dc23167c8d8 45
pHysiX 6:2dc23167c8d8 46 /** Read a single bit from a 16-bit device register.
pHysiX 6:2dc23167c8d8 47 * @param devAddr I2C slave device address
pHysiX 6:2dc23167c8d8 48 * @param regAddr Register regAddr to read from
pHysiX 6:2dc23167c8d8 49 * @param bitNum Bit position to read (0-15)
pHysiX 6:2dc23167c8d8 50 * @param data Container for single bit value
pHysiX 6:2dc23167c8d8 51 * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
pHysiX 6:2dc23167c8d8 52 * @return Status of read operation (true = success)
pHysiX 6:2dc23167c8d8 53 */
pHysiX 6:2dc23167c8d8 54 int8_t I2Cdev::readBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t *data, uint16_t timeout)
pHysiX 6:2dc23167c8d8 55 {
pHysiX 6:2dc23167c8d8 56 uint16_t b = 0x0000;
pHysiX 6:2dc23167c8d8 57 uint8_t count = readWord(devAddr, regAddr, &b, timeout);
pHysiX 6:2dc23167c8d8 58 *data = b & (1 << bitNum);
pHysiX 6:2dc23167c8d8 59 return count;
pHysiX 6:2dc23167c8d8 60 }
pHysiX 6:2dc23167c8d8 61
pHysiX 6:2dc23167c8d8 62 /** Read multiple bits from an 8-bit device register.
pHysiX 6:2dc23167c8d8 63 * @param devAddr I2C slave device address
pHysiX 6:2dc23167c8d8 64 * @param regAddr Register regAddr to read from
pHysiX 6:2dc23167c8d8 65 * @param bitStart First bit position to read (0-7)
pHysiX 6:2dc23167c8d8 66 * @param length Number of bits to read (not more than 8)
pHysiX 6:2dc23167c8d8 67 * @param data Container for right-aligned value (i.e. '101' read from any bitStart position will equal 0x05)
pHysiX 6:2dc23167c8d8 68 * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
pHysiX 6:2dc23167c8d8 69 * @return Status of read operation (true = success)
pHysiX 6:2dc23167c8d8 70 */
pHysiX 6:2dc23167c8d8 71 int8_t I2Cdev::readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data, uint16_t timeout)
pHysiX 6:2dc23167c8d8 72 {
pHysiX 6:2dc23167c8d8 73 // 01101001 read byte
pHysiX 6:2dc23167c8d8 74 // 76543210 bit numbers
pHysiX 6:2dc23167c8d8 75 // xxx args: bitStart=4, length=3
pHysiX 6:2dc23167c8d8 76 // 010 masked
pHysiX 6:2dc23167c8d8 77 // -> 010 shifted
pHysiX 6:2dc23167c8d8 78 uint8_t count, b;
pHysiX 6:2dc23167c8d8 79 if ((count = readByte(devAddr, regAddr, &b, timeout)) != 0) {
pHysiX 6:2dc23167c8d8 80 uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1);
pHysiX 6:2dc23167c8d8 81 b &= mask;
pHysiX 6:2dc23167c8d8 82 b >>= (bitStart - length + 1);
pHysiX 6:2dc23167c8d8 83 *data = b;
pHysiX 6:2dc23167c8d8 84 }
pHysiX 6:2dc23167c8d8 85 return count;
pHysiX 6:2dc23167c8d8 86 }
pHysiX 6:2dc23167c8d8 87
pHysiX 6:2dc23167c8d8 88 /** Read multiple bits from a 16-bit device register.
pHysiX 6:2dc23167c8d8 89 * @param devAddr I2C slave device address
pHysiX 6:2dc23167c8d8 90 * @param regAddr Register regAddr to read from
pHysiX 6:2dc23167c8d8 91 * @param bitStart First bit position to read (0-15)
pHysiX 6:2dc23167c8d8 92 * @param length Number of bits to read (not more than 16)
pHysiX 6:2dc23167c8d8 93 * @param data Container for right-aligned value (i.e. '101' read from any bitStart position will equal 0x05)
pHysiX 6:2dc23167c8d8 94 * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
pHysiX 6:2dc23167c8d8 95 * @return Status of read operation (1 = success, 0 = failure, -1 = timeout)
pHysiX 6:2dc23167c8d8 96 */
pHysiX 6:2dc23167c8d8 97 int8_t I2Cdev::readBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t *data, uint16_t timeout)
pHysiX 6:2dc23167c8d8 98 {
pHysiX 6:2dc23167c8d8 99 // 1101011001101001 read byte
pHysiX 6:2dc23167c8d8 100 // fedcba9876543210 bit numbers
pHysiX 6:2dc23167c8d8 101 // xxx args: bitStart=12, length=3
pHysiX 6:2dc23167c8d8 102 // 010 masked
pHysiX 6:2dc23167c8d8 103 // -> 010 shifted
pHysiX 6:2dc23167c8d8 104 uint8_t count;
pHysiX 6:2dc23167c8d8 105 uint16_t w;
pHysiX 6:2dc23167c8d8 106 if ((count = readWord(devAddr, regAddr, &w, timeout)) != 0) {
pHysiX 6:2dc23167c8d8 107 uint16_t mask = ((1 << length) - 1) << (bitStart - length + 1);
pHysiX 6:2dc23167c8d8 108 w &= mask;
pHysiX 6:2dc23167c8d8 109 w >>= (bitStart - length + 1);
pHysiX 6:2dc23167c8d8 110 *data = w;
pHysiX 6:2dc23167c8d8 111 }
pHysiX 6:2dc23167c8d8 112 return count;
pHysiX 6:2dc23167c8d8 113 }
pHysiX 6:2dc23167c8d8 114 /** Read single byte from an 8-bit device register.
pHysiX 6:2dc23167c8d8 115 * @param devAddr I2C slave device address
pHysiX 6:2dc23167c8d8 116 * @param regAddr Register regAddr to read from
pHysiX 6:2dc23167c8d8 117 * @param data Container for byte value read from device
pHysiX 6:2dc23167c8d8 118 * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
pHysiX 6:2dc23167c8d8 119 * @return Status of read operation (true = success)
pHysiX 6:2dc23167c8d8 120 */
pHysiX 6:2dc23167c8d8 121 int8_t I2Cdev::readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data, uint16_t timeout)
pHysiX 6:2dc23167c8d8 122 {
pHysiX 6:2dc23167c8d8 123 return readBytes(devAddr, regAddr, 1, data, timeout);
pHysiX 6:2dc23167c8d8 124 }
pHysiX 6:2dc23167c8d8 125
pHysiX 6:2dc23167c8d8 126 /** Read single word from a 16-bit device register.
pHysiX 6:2dc23167c8d8 127 * @param devAddr I2C slave device address
pHysiX 6:2dc23167c8d8 128 * @param regAddr Register regAddr to read from
pHysiX 6:2dc23167c8d8 129 * @param data Container for word value read from device
pHysiX 6:2dc23167c8d8 130 * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
pHysiX 6:2dc23167c8d8 131 * @return Status of read operation (true = success)
pHysiX 6:2dc23167c8d8 132 */
pHysiX 6:2dc23167c8d8 133 int8_t I2Cdev::readWord(uint8_t devAddr, uint8_t regAddr, uint16_t *data, uint16_t timeout)
pHysiX 6:2dc23167c8d8 134 {
pHysiX 6:2dc23167c8d8 135 return readWords(devAddr, regAddr, 1, data, timeout);
pHysiX 6:2dc23167c8d8 136 }
pHysiX 6:2dc23167c8d8 137
pHysiX 6:2dc23167c8d8 138 /** Read multiple bytes from an 8-bit device register.
pHysiX 6:2dc23167c8d8 139 * @param devAddr I2C slave device address
pHysiX 6:2dc23167c8d8 140 * @param regAddr First register regAddr to read from
pHysiX 6:2dc23167c8d8 141 * @param length Number of bytes to read
pHysiX 6:2dc23167c8d8 142 * @param data Buffer to store read data in
pHysiX 6:2dc23167c8d8 143 * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
pHysiX 6:2dc23167c8d8 144 * @return Number of bytes read (-1 indicates failure)
pHysiX 6:2dc23167c8d8 145 */
pHysiX 6:2dc23167c8d8 146 int8_t I2Cdev::readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, uint16_t timeout)
pHysiX 6:2dc23167c8d8 147 {
pHysiX 6:2dc23167c8d8 148 char command[1];
pHysiX 6:2dc23167c8d8 149 command[0] = regAddr;
pHysiX 6:2dc23167c8d8 150 char *redData = (char*)malloc(length);
pHysiX 6:2dc23167c8d8 151 i2c.write(devAddr<<1, command, 1, true);
akashvibhute 15:d4284f5b4fc7 152 i2c.read(devAddr<<1, redData, length, false);
akashvibhute 15:d4284f5b4fc7 153 for(int i =0; i < length; i++)
akashvibhute 15:d4284f5b4fc7 154 {
pHysiX 6:2dc23167c8d8 155 data[i] = redData[i];
pHysiX 6:2dc23167c8d8 156 }
pHysiX 6:2dc23167c8d8 157 free (redData);
pHysiX 6:2dc23167c8d8 158 return length;
pHysiX 6:2dc23167c8d8 159 }
pHysiX 6:2dc23167c8d8 160
pHysiX 6:2dc23167c8d8 161 int8_t I2Cdev::readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data, uint16_t timeout)
pHysiX 6:2dc23167c8d8 162 {
pHysiX 6:2dc23167c8d8 163 return 0;
pHysiX 6:2dc23167c8d8 164 }
pHysiX 6:2dc23167c8d8 165
pHysiX 6:2dc23167c8d8 166 /** write a single bit in an 8-bit device register.
pHysiX 6:2dc23167c8d8 167 * @param devAddr I2C slave device address
pHysiX 6:2dc23167c8d8 168 * @param regAddr Register regAddr to write to
pHysiX 6:2dc23167c8d8 169 * @param bitNum Bit position to write (0-7)
pHysiX 6:2dc23167c8d8 170 * @param value New bit value to write
pHysiX 6:2dc23167c8d8 171 * @return Status of operation (true = success)
pHysiX 6:2dc23167c8d8 172 */
pHysiX 6:2dc23167c8d8 173 bool I2Cdev::writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data)
pHysiX 6:2dc23167c8d8 174 {
pHysiX 6:2dc23167c8d8 175 uint8_t b;
pHysiX 6:2dc23167c8d8 176 readByte(devAddr, regAddr, &b);
pHysiX 6:2dc23167c8d8 177 b = (data != 0) ? (b | (1 << bitNum)) : (b & ~(1 << bitNum));
pHysiX 6:2dc23167c8d8 178 return writeByte(devAddr, regAddr, b);
pHysiX 6:2dc23167c8d8 179 }
pHysiX 6:2dc23167c8d8 180
pHysiX 6:2dc23167c8d8 181 /** write a single bit in a 16-bit device register.
pHysiX 6:2dc23167c8d8 182 * @param devAddr I2C slave device address
pHysiX 6:2dc23167c8d8 183 * @param regAddr Register regAddr to write to
pHysiX 6:2dc23167c8d8 184 * @param bitNum Bit position to write (0-15)
pHysiX 6:2dc23167c8d8 185 * @param value New bit value to write
pHysiX 6:2dc23167c8d8 186 * @return Status of operation (true = success)
pHysiX 6:2dc23167c8d8 187 */
pHysiX 6:2dc23167c8d8 188 bool I2Cdev::writeBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t data)
pHysiX 6:2dc23167c8d8 189 {
pHysiX 6:2dc23167c8d8 190 uint16_t w;
pHysiX 6:2dc23167c8d8 191 readWord(devAddr, regAddr, &w);
pHysiX 6:2dc23167c8d8 192 w = (data != 0) ? (w | (1 << bitNum)) : (w & ~(1 << bitNum));
pHysiX 6:2dc23167c8d8 193 return writeWord(devAddr, regAddr, w);
pHysiX 6:2dc23167c8d8 194 }
pHysiX 6:2dc23167c8d8 195
pHysiX 6:2dc23167c8d8 196 /** Write multiple bits in an 8-bit device register.
pHysiX 6:2dc23167c8d8 197 * @param devAddr I2C slave device address
pHysiX 6:2dc23167c8d8 198 * @param regAddr Register regAddr to write to
pHysiX 6:2dc23167c8d8 199 * @param bitStart First bit position to write (0-7)
pHysiX 6:2dc23167c8d8 200 * @param length Number of bits to write (not more than 8)
pHysiX 6:2dc23167c8d8 201 * @param data Right-aligned value to write
pHysiX 6:2dc23167c8d8 202 * @return Status of operation (true = success)
pHysiX 6:2dc23167c8d8 203 */
pHysiX 6:2dc23167c8d8 204 bool I2Cdev::writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data)
pHysiX 6:2dc23167c8d8 205 {
pHysiX 6:2dc23167c8d8 206 // 010 value to write
pHysiX 6:2dc23167c8d8 207 // 76543210 bit numbers
pHysiX 6:2dc23167c8d8 208 // xxx args: bitStart=4, length=3
pHysiX 6:2dc23167c8d8 209 // 00011100 mask byte
pHysiX 6:2dc23167c8d8 210 // 10101111 original value (sample)
pHysiX 6:2dc23167c8d8 211 // 10100011 original & ~mask
pHysiX 6:2dc23167c8d8 212 // 10101011 masked | value
pHysiX 6:2dc23167c8d8 213 uint8_t b;
pHysiX 6:2dc23167c8d8 214 if (readByte(devAddr, regAddr, &b) != 0) {
pHysiX 6:2dc23167c8d8 215 uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1);
pHysiX 6:2dc23167c8d8 216 data <<= (bitStart - length + 1); // shift data into correct position
pHysiX 6:2dc23167c8d8 217 data &= mask; // zero all non-important bits in data
pHysiX 6:2dc23167c8d8 218 b &= ~(mask); // zero all important bits in existing byte
pHysiX 6:2dc23167c8d8 219 b |= data; // combine data with existing byte
pHysiX 6:2dc23167c8d8 220 return writeByte(devAddr, regAddr, b);
pHysiX 6:2dc23167c8d8 221 } else {
pHysiX 6:2dc23167c8d8 222 return false;
pHysiX 6:2dc23167c8d8 223 }
pHysiX 6:2dc23167c8d8 224 }
pHysiX 6:2dc23167c8d8 225
pHysiX 6:2dc23167c8d8 226 /** Write multiple bits in a 16-bit device register.
pHysiX 6:2dc23167c8d8 227 * @param devAddr I2C slave device address
pHysiX 6:2dc23167c8d8 228 * @param regAddr Register regAddr to write to
pHysiX 6:2dc23167c8d8 229 * @param bitStart First bit position to write (0-15)
pHysiX 6:2dc23167c8d8 230 * @param length Number of bits to write (not more than 16)
pHysiX 6:2dc23167c8d8 231 * @param data Right-aligned value to write
pHysiX 6:2dc23167c8d8 232 * @return Status of operation (true = success)
pHysiX 6:2dc23167c8d8 233 */
pHysiX 6:2dc23167c8d8 234 bool I2Cdev::writeBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t data)
pHysiX 6:2dc23167c8d8 235 {
pHysiX 6:2dc23167c8d8 236 // 010 value to write
pHysiX 6:2dc23167c8d8 237 // fedcba9876543210 bit numbers
pHysiX 6:2dc23167c8d8 238 // xxx args: bitStart=12, length=3
pHysiX 6:2dc23167c8d8 239 // 0001110000000000 mask byte
pHysiX 6:2dc23167c8d8 240 // 1010111110010110 original value (sample)
pHysiX 6:2dc23167c8d8 241 // 1010001110010110 original & ~mask
pHysiX 6:2dc23167c8d8 242 // 1010101110010110 masked | value
pHysiX 6:2dc23167c8d8 243 uint16_t w;
pHysiX 6:2dc23167c8d8 244 if (readWord(devAddr, regAddr, &w) != 0) {
pHysiX 6:2dc23167c8d8 245 uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1);
pHysiX 6:2dc23167c8d8 246 data <<= (bitStart - length + 1); // shift data into correct position
pHysiX 6:2dc23167c8d8 247 data &= mask; // zero all non-important bits in data
pHysiX 6:2dc23167c8d8 248 w &= ~(mask); // zero all important bits in existing word
pHysiX 6:2dc23167c8d8 249 w |= data; // combine data with existing word
pHysiX 6:2dc23167c8d8 250 return writeWord(devAddr, regAddr, w);
pHysiX 6:2dc23167c8d8 251 } else {
pHysiX 6:2dc23167c8d8 252 return false;
pHysiX 6:2dc23167c8d8 253 }
pHysiX 6:2dc23167c8d8 254 }
pHysiX 6:2dc23167c8d8 255
pHysiX 6:2dc23167c8d8 256 /** Write single byte to an 8-bit device register.
pHysiX 6:2dc23167c8d8 257 * @param devAddr I2C slave device address
pHysiX 6:2dc23167c8d8 258 * @param regAddr Register address to write to
pHysiX 6:2dc23167c8d8 259 * @param data New byte value to write
pHysiX 6:2dc23167c8d8 260 * @return Status of operation (true = success)
pHysiX 6:2dc23167c8d8 261 */
pHysiX 6:2dc23167c8d8 262 bool I2Cdev::writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data)
pHysiX 6:2dc23167c8d8 263 {
pHysiX 6:2dc23167c8d8 264 return writeBytes(devAddr, regAddr, 1, &data);
pHysiX 6:2dc23167c8d8 265 }
pHysiX 6:2dc23167c8d8 266
pHysiX 6:2dc23167c8d8 267 /** Write single word to a 16-bit device register.
pHysiX 6:2dc23167c8d8 268 * @param devAddr I2C slave device address
pHysiX 6:2dc23167c8d8 269 * @param regAddr Register address to write to
pHysiX 6:2dc23167c8d8 270 * @param data New word value to write
pHysiX 6:2dc23167c8d8 271 * @return Status of operation (true = success)
pHysiX 6:2dc23167c8d8 272 */
pHysiX 6:2dc23167c8d8 273 bool I2Cdev::writeWord(uint8_t devAddr, uint8_t regAddr, uint16_t data)
pHysiX 6:2dc23167c8d8 274 {
pHysiX 6:2dc23167c8d8 275 return writeWords(devAddr, regAddr, 1, &data);
pHysiX 6:2dc23167c8d8 276 }
pHysiX 6:2dc23167c8d8 277
pHysiX 6:2dc23167c8d8 278 bool I2Cdev::writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data)
pHysiX 6:2dc23167c8d8 279 {
akashvibhute 15:d4284f5b4fc7 280 char reg_addrs[1] = {regAddr};
akashvibhute 15:d4284f5b4fc7 281 char data_w[length];
akashvibhute 15:d4284f5b4fc7 282 for(int i=0; i<length; i++)
akashvibhute 15:d4284f5b4fc7 283 data_w[i] = data[i];
akashvibhute 15:d4284f5b4fc7 284
akashvibhute 15:d4284f5b4fc7 285 i2c.write(devAddr<<1, reg_addrs, 1, true);
akashvibhute 15:d4284f5b4fc7 286
akashvibhute 15:d4284f5b4fc7 287 for(int i = 0; i < length; i++)
akashvibhute 15:d4284f5b4fc7 288 {
akashvibhute 15:d4284f5b4fc7 289 if(!i2c.write(data_w[i]))
akashvibhute 15:d4284f5b4fc7 290 {
akashvibhute 15:d4284f5b4fc7 291 //debug.printf("Write Error %d\r\n", reg_addr);
akashvibhute 15:d4284f5b4fc7 292 return false;
akashvibhute 15:d4284f5b4fc7 293 }
pHysiX 6:2dc23167c8d8 294 }
pHysiX 6:2dc23167c8d8 295 i2c.stop();
akashvibhute 15:d4284f5b4fc7 296
pHysiX 6:2dc23167c8d8 297 return true;
pHysiX 6:2dc23167c8d8 298 }
pHysiX 6:2dc23167c8d8 299
pHysiX 6:2dc23167c8d8 300 bool I2Cdev::writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data)
pHysiX 6:2dc23167c8d8 301 {
akashvibhute 16:babac099274f 302 //uint8_t data8[2];
akashvibhute 16:babac099274f 303
akashvibhute 16:babac099274f 304 //data8[1] = (uint8_t)(data[0]); //low bits
akashvibhute 16:babac099274f 305 //data8[0] = (uint8_t)(data[0] >> 8); //high bits
akashvibhute 16:babac099274f 306
akashvibhute 16:babac099274f 307 char reg_addrs[1] = {regAddr};
akashvibhute 16:babac099274f 308 i2c.write(devAddr<<1, reg_addrs, 1, true);
akashvibhute 16:babac099274f 309
akashvibhute 16:babac099274f 310 for(int i = 0; i < length; i++)
akashvibhute 16:babac099274f 311 {
akashvibhute 16:babac099274f 312 if(!i2c.write((uint8_t)(data[i] >> 8)) )
akashvibhute 16:babac099274f 313 {
akashvibhute 16:babac099274f 314 //debug.printf("Write Error %d\r\n", reg_addr);
akashvibhute 16:babac099274f 315 return false;
akashvibhute 16:babac099274f 316 }
akashvibhute 16:babac099274f 317
akashvibhute 16:babac099274f 318 if(!i2c.write((uint8_t)(data[i])) )
akashvibhute 16:babac099274f 319 {
akashvibhute 16:babac099274f 320 //debug.printf("Write Error %d\r\n", reg_addr);
akashvibhute 16:babac099274f 321 return false;
akashvibhute 16:babac099274f 322 }
akashvibhute 16:babac099274f 323 }
akashvibhute 16:babac099274f 324 i2c.stop();
akashvibhute 16:babac099274f 325
pHysiX 6:2dc23167c8d8 326 return true;
pHysiX 6:2dc23167c8d8 327 }
pHysiX 6:2dc23167c8d8 328
pHysiX 6:2dc23167c8d8 329 uint16_t I2Cdev::readTimeout(void)
pHysiX 6:2dc23167c8d8 330 {
pHysiX 6:2dc23167c8d8 331 return 0;
pHysiX 7:4619a083f289 332 }