1 | const uint8_t IMUAddress = 0x68; // AD0 is logic low on the PCB
|
2 | const uint16_t I2C_TIMEOUT = 1000; // Used to check for errors in I2C communication
|
3 |
|
4 | uint8_t i2cWrite(uint8_t registerAddress, uint8_t data, bool sendStop) {
|
5 | return i2cWrite(registerAddress, &data, 1, sendStop); // Returns 0 on success
|
6 | }
|
7 |
|
8 | uint8_t i2cWrite(uint8_t registerAddress, uint8_t *data, uint8_t length, bool sendStop) {
|
9 | Wire.beginTransmission(IMUAddress);
|
10 | Wire.write(registerAddress);
|
11 | Wire.write(data, length);
|
12 | uint8_t rcode = Wire.endTransmission(sendStop); // Returns 0 on success
|
13 | if (rcode) {
|
14 | Serial.print(F("i2cWrite failed: "));
|
15 | Serial.println(rcode);
|
16 | }
|
17 | return rcode; // See: http://arduino.cc/en/Reference/WireEndTransmission
|
18 | }
|
19 |
|
20 | uint8_t i2cRead(uint8_t registerAddress, uint8_t *data, uint8_t nbytes) {
|
21 | uint32_t timeOutTimer;
|
22 | Wire.beginTransmission(IMUAddress);
|
23 | Wire.write(registerAddress);
|
24 | uint8_t rcode = Wire.endTransmission(false); // Don't release the bus
|
25 | if (rcode) {
|
26 | Serial.print(F("i2cRead failed: "));
|
27 | Serial.println(rcode);
|
28 | return rcode; // See: http://arduino.cc/en/Reference/WireEndTransmission
|
29 | }
|
30 | Wire.requestFrom(IMUAddress, nbytes, (uint8_t)true); // Send a repeated start and then release the bus after reading
|
31 | for (uint8_t i = 0; i < nbytes; i++) {
|
32 | if (Wire.available())
|
33 | data[i] = Wire.read();
|
34 | else {
|
35 | timeOutTimer = micros();
|
36 | while (((micros() - timeOutTimer) < I2C_TIMEOUT) && !Wire.available());
|
37 | if (Wire.available())
|
38 | data[i] = Wire.read();
|
39 | else {
|
40 | Serial.println(F("i2cRead timeout"));
|
41 | return 5; // This error value is not already taken by endTransmission
|
42 | }
|
43 | }
|
44 | }
|
45 | return 0; // Success
|
46 | }
|
