Skip to content
Browse files

Updating I2Cdev class and various devices

  • Loading branch information...
1 parent 51d0ab1 commit fc92547377028a63489c44cfc689355059fdf3c5 @jrowberg committed Sep 2, 2013
View
8 Arduino/ADXL345/ADXL345.cpp
@@ -1642,11 +1642,11 @@ void ADXL345::setFIFOSamples(uint8_t size) {
* A 1 in the FIFO_TRIG bit corresponds to a trigger event occurring, and a 0
* means that a FIFO trigger event has not occurred.
* @return FIFO trigger occurred status
- * @see ADXL345_RA_FIFO_CTL
+ * @see ADXL345_RA_FIFO_STATUS
* @see ADXL345_FIFOSTAT_TRIGGER_BIT
*/
bool ADXL345::getFIFOTriggerOccurred() {
- I2Cdev::readBit(devAddr, ADXL345_RA_FIFO_CTL, ADXL345_FIFOSTAT_TRIGGER_BIT, buffer);
+ I2Cdev::readBit(devAddr, ADXL345_RA_FIFO_STATUS, ADXL345_FIFOSTAT_TRIGGER_BIT, buffer);
return buffer[0];
}
/** Get FIFO length.
@@ -1658,11 +1658,11 @@ bool ADXL345::getFIFOTriggerOccurred() {
* available at any given time because an additional entry is available at the
* output filter of the I2Cdev::
* @return Current FIFO length
- * @see ADXL345_RA_FIFO_CTL
+ * @see ADXL345_RA_FIFO_STATUS
* @see ADXL345_FIFOSTAT_LENGTH_BIT
* @see ADXL345_FIFOSTAT_LENGTH_LENGTH
*/
uint8_t ADXL345::getFIFOLength() {
- I2Cdev::readBits(devAddr, ADXL345_RA_FIFO_CTL, ADXL345_FIFOSTAT_LENGTH_BIT, ADXL345_FIFOSTAT_LENGTH_LENGTH, buffer);
+ I2Cdev::readBits(devAddr, ADXL345_RA_FIFO_STATUS, ADXL345_FIFOSTAT_LENGTH_BIT, ADXL345_FIFOSTAT_LENGTH_LENGTH, buffer);
return buffer[0];
}
View
2 Arduino/AK8975/Examples/AK8975_MPUEVB_heading/AK8975_MPUEVB_heading.ino
@@ -60,7 +60,7 @@ THE SOFTWARE.
// Addr pins low/high = 0x0D
// Addr pins high/low = 0x0E (default for InvenSense MPU6050 evaluation board)
// Addr pins high/high = 0x0F
-AK8975 mag(0x0E);
+AK8975 mag(0x0C);
MPU6050 accelgyro; // address = 0x68, the default, on MPU6050 EVB
int16_t mx, my, mz;
View
228 Arduino/I2Cdev/I2Cdev.cpp
@@ -3,25 +3,25 @@
// 6/9/2012 by Jeff Rowberg <jeff@rowberg.net>
//
// Changelog:
-// 2013-05-05 - fix issue with writing bit values to words (Sasquatch/Farzanegan)
-// 2012-06-09 - fix major issue with reading > 32 bytes at a time with Arduino Wire
-// - add compiler warnings when using outdated or IDE or limited I2Cdev implementation
-// 2011-11-01 - fix write*Bits mask calculation (thanks sasquatch @ Arduino forums)
-// 2011-10-03 - added automatic Arduino version detection for ease of use
-// 2011-10-02 - added Gene Knight's NBWire TwoWire class implementation with small modifications
-// 2011-08-31 - added support for Arduino 1.0 Wire library (methods are different from 0.x)
-// 2011-08-03 - added optional timeout parameter to read* methods to easily change from default
-// 2011-08-02 - added support for 16-bit registers
-// - fixed incorrect Doxygen comments on some methods
-// - added timeout value for read operations (thanks mem @ Arduino forums)
-// 2011-07-30 - changed read/write function structures to return success or byte counts
-// - made all methods static for multi-device memory savings
-// 2011-07-28 - initial release
-
+// 2013-05-06 - add Francesco Ferrara's Fastwire v0.24 implementation with small modifications
+// 2013-05-05 - fix issue with writing bit values to words (Sasquatch/Farzanegan)
+// 2012-06-09 - fix major issue with reading > 32 bytes at a time with Arduino Wire
+// - add compiler warnings when using outdated or IDE or limited I2Cdev implementation
+// 2011-11-01 - fix write*Bits mask calculation (thanks sasquatch @ Arduino forums)
+// 2011-10-03 - added automatic Arduino version detection for ease of use
+// 2011-10-02 - added Gene Knight's NBWire TwoWire class implementation with small modifications
+// 2011-08-31 - added support for Arduino 1.0 Wire library (methods are different from 0.x)
+// 2011-08-03 - added optional timeout parameter to read* methods to easily change from default
+// 2011-08-02 - added support for 16-bit registers
+// - fixed incorrect Doxygen comments on some methods
+// - added timeout value for read operations (thanks mem @ Arduino forums)
+// 2011-07-30 - changed read/write function structures to return success or byte counts
+// - made all methods static for multi-device memory savings
+// 2011-07-28 - initial release
/* ============================================
I2Cdev device library code is placed under the MIT license
-Copyright (c) 2012 Jeff Rowberg
+Copyright (c) 2013 Jeff Rowberg
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
@@ -61,18 +61,16 @@ THE SOFTWARE.
#warning - Repeated starts conditions
#warning - Timeout detection (some Wire requests block forever)
#elif ARDUINO > 100
- /*
#warning Using current Arduino IDE with Wire library is functionally limiting.
#warning Arduino IDE v1.0.1+ with I2CDEV_BUILTIN_FASTWIRE implementation is recommended.
#warning This I2Cdev implementation does not support:
#warning - Timeout detection (some Wire requests block forever)
- */
#endif
#endif
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
- #error The I2CDEV_BUILTIN_FASTWIRE implementation is known to be broken right now. Patience, Iago!
+ //#error The I2CDEV_BUILTIN_FASTWIRE implementation is known to be broken right now. Patience, Iago!
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE
@@ -87,10 +85,6 @@ THE SOFTWARE.
// Originally offered to the i2cdevlib project at http://arduino.cc/forum/index.php/topic,68210.30.html
TwoWire Wire;
-#elif I2CDEV_IMPLEMENTATION == I2CDEV_I2CMASTER_LIBRARY
-
- #warning Dunno, just don't want it to feel left out ^_^'
-
#endif
/** Default constructor.
@@ -293,29 +287,22 @@ int8_t I2Cdev::readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8
if (count + 1 < length) Serial.print(" ");
#endif
}
+
+ Wire.endTransmission();
}
#endif
#elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE)
- // Fastwire library (STILL UNDER DEVELOPMENT, NON-FUNCTIONAL!)
+ // Fastwire library
// no loop required for fastwire
- uint8_t status = Fastwire::readBuf(devAddr, regAddr, data, length);
+ uint8_t status = Fastwire::readBuf(devAddr << 1, regAddr, data, length);
if (status == 0) {
count = length; // success
} else {
count = -1; // error
}
- #elif (I2CDEV_IMPLEMENTATION == I2CDEV_I2CMASTER_LIBRARY)
-
- uint8_t status = I2c.read(devAddr, regAddr, length, data);
- if(status == 0) {
- count = length;
- } else {
- count = -1 * status;
- }
-
#endif
// check for timeout
@@ -336,7 +323,7 @@ int8_t I2Cdev::readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8
* @param length Number of words to read
* @param data Buffer to store read data in
* @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
- * @return Number of words read (negative value indicates failure)
+ * @return Number of words read (0 indicates failure)
*/
int8_t I2Cdev::readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data, uint16_t timeout) {
#ifdef I2CDEV_SERIAL_DEBUG
@@ -449,15 +436,17 @@ int8_t I2Cdev::readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint1
}
msb = !msb;
}
+
+ Wire.endTransmission();
}
#endif
#elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE)
- // Fastwire library (STILL UNDER DEVELOPMENT, NON-FUNCTIONAL!)
+ // Fastwire library
// no loop required for fastwire
uint16_t intermediate[(uint8_t)length];
- uint8_t status = Fastwire::readBuf(devAddr, regAddr, (uint8_t *)intermediate, (uint8_t)(length * 2));
+ uint8_t status = Fastwire::readBuf(devAddr << 1, regAddr, (uint8_t *)intermediate, (uint8_t)(length * 2));
if (status == 0) {
count = length; // success
for (uint8_t i = 0; i < length; i++) {
@@ -467,18 +456,6 @@ int8_t I2Cdev::readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint1
count = -1; // error
}
- #elif (I2CDEV_IMPLEMENTATION == I2CDEV_I2CMASTER_LIBRARY)
-
- uint16_t intermediate[(uint8_t)length];
- uint8_t status = I2c.read(devAddr, regAddr, length*2, (uint8_t *)intermediate);
- if(status == 0) {
- count = length;
- for(uint8_t i = 0; i < length; i++) {
- data[i] = (intermediate[2*i] << 8) | intermediate[2*i + i];
- }
- } else {
- count = -1 * status;
- }
#endif
if (timeout > 0 && millis() - t1 >= timeout && count < length) count = -1; // timeout
@@ -622,27 +599,30 @@ bool I2Cdev::writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_
#elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100)
Wire.beginTransmission(devAddr);
Wire.write((uint8_t) regAddr); // send address
+ #elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE)
+ Fastwire::beginTransmission(devAddr);
+ Fastwire::write(regAddr);
#endif
for (uint8_t i = 0; i < length; i++) {
+ #ifdef I2CDEV_SERIAL_DEBUG
+ Serial.print(data[i], HEX);
+ if (i + 1 < length) Serial.print(" ");
+ #endif
#if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE)
Wire.send((uint8_t) data[i]);
#elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100)
Wire.write((uint8_t) data[i]);
#elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE)
- status = Fastwire::write(devAddr, regAddr, data[i]);
- Serial.println(status);
- #elif (I2CDEV_IMPLEMENTATION == I2CDEV_I2CMASTER_LIBRARY)
- status = I2c.write(devAddr, regAddr, data[i]);
- #endif
- #ifdef I2CDEV_SERIAL_DEBUG
- Serial.print(data[i], HEX);
- if (i + 1 < length) Serial.print(" ");
+ Fastwire::write((uint8_t) data[i]);
#endif
}
#if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE)
Wire.endTransmission();
#elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100)
status = Wire.endTransmission();
+ #elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE)
+ Fastwire::stop();
+ //status = Fastwire::endTransmission();
#endif
#ifdef I2CDEV_SERIAL_DEBUG
Serial.println(". Done.");
@@ -674,30 +654,34 @@ bool I2Cdev::writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16
#elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100)
Wire.beginTransmission(devAddr);
Wire.write(regAddr); // send address
+ #elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE)
+ Fastwire::beginTransmission(devAddr);
+ Fastwire::write(regAddr);
#endif
for (uint8_t i = 0; i < length * 2; i++) {
- #if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE)
- Wire.send((uint8_t)(data[i++] >> 8)); // send MSB
- Wire.send((uint8_t)data[i]); // send LSB
- #elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100)
- Wire.write((uint8_t)(data[i++] >> 8)); // send MSB
- Wire.write((uint8_t)data[i]); // send LSB
- #elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE)
- status = Fastwire::write(devAddr, regAddr, (uint8_t)(data[i++] >> 8));
- status = Fastwire::write(devAddr, regAddr + 1, (uint8_t)data[i]);
- #elif (I2CDEV_IMPLEMENTATION == I2CDEV_I2CMASTER_LIBRARY)
- status = I2c.write((uint8_t)devAddr, (uint8_t)regAddr, (uint8_t)(data[i++] >> 8));
- status = I2c.write((uint8_t)devAddr, (uint8_t)regAddr + 1, (uint8_t)data[i]);
- #endif
#ifdef I2CDEV_SERIAL_DEBUG
Serial.print(data[i], HEX);
if (i + 1 < length) Serial.print(" ");
#endif
+ #if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE)
+ Wire.send((uint8_t)(data[i] >> 8)); // send MSB
+ Wire.send((uint8_t)data[i++]); // send LSB
+ #elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100)
+ Wire.write((uint8_t)(data[i] >> 8)); // send MSB
+ Wire.write((uint8_t)data[i++]); // send LSB
+ #elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE)
+ Fastwire::write((uint8_t)(data[i] >> 8)); // send MSB
+ status = Fastwire::write((uint8_t)data[i++]); // send LSB
+ if (status != 0) break;
+ #endif
}
#if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE)
Wire.endTransmission();
#elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100)
status = Wire.endTransmission();
+ #elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE)
+ Fastwire::stop();
+ //status = Fastwire::endTransmission();
#endif
#ifdef I2CDEV_SERIAL_DEBUG
Serial.println(". Done.");
@@ -711,12 +695,23 @@ bool I2Cdev::writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16
uint16_t I2Cdev::readTimeout = I2CDEV_DEFAULT_READ_TIMEOUT;
#if I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
+ // I2C library
+ //////////////////////
+ // Copyright(C) 2012
+ // Francesco Ferrara
+ // ferrara[at]libero[point]it
+ //////////////////////
+
/*
- FastWire 0.2
- This is a library to help faster programs to read I2C devices.
- Copyright(C) 2011 Francesco Ferrara
- occhiobello at gmail dot com
- */
+ FastWire
+ - 0.24 added stop
+ - 0.23 added reset
+
+ This is a library to help faster programs to read I2C devices.
+ Copyright(C) 2012 Francesco Ferrara
+ occhiobello at gmail dot com
+ [used by Jeff Rowberg for I2Cdevlib with permission]
+ */
boolean Fastwire::waitInt() {
int l = 250;
@@ -747,7 +742,30 @@ uint16_t I2Cdev::readTimeout = I2CDEV_DEFAULT_READ_TIMEOUT;
TWCR = 1 << TWEN; // enable twi module, no interrupt
}
- byte Fastwire::write(byte device, byte address, byte value) {
+ // added by Jeff Rowberg 2013-05-07:
+ // Arduino Wire-style "beginTransmission" function
+ // (takes 7-bit device address like the Wire method, NOT 8-bit: 0x68, not 0xD0/0xD1)
+ byte Fastwire::beginTransmission(byte device) {
+ byte twst, retry;
+ retry = 2;
+ do {
+ TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWSTO) | (1 << TWSTA);
+ if (!waitInt()) return 1;
+ twst = TWSR & 0xF8;
+ if (twst != TW_START && twst != TW_REP_START) return 2;
+
+ //Serial.print(device, HEX);
+ //Serial.print(" ");
+ TWDR = device << 1; // send device address without read bit (1)
+ TWCR = (1 << TWINT) | (1 << TWEN);
+ if (!waitInt()) return 3;
+ twst = TWSR & 0xF8;
+ } while (twst == TW_MT_SLA_NACK && retry-- > 0);
+ if (twst != TW_MT_SLA_ACK) return 4;
+ return 0;
+ }
+
+ byte Fastwire::writeBuf(byte device, byte address, byte *data, byte num) {
byte twst, retry;
retry = 2;
@@ -757,25 +775,45 @@ uint16_t I2Cdev::readTimeout = I2CDEV_DEFAULT_READ_TIMEOUT;
twst = TWSR & 0xF8;
if (twst != TW_START && twst != TW_REP_START) return 2;
+ //Serial.print(device, HEX);
+ //Serial.print(" ");
TWDR = device & 0xFE; // send device address without read bit (1)
TWCR = (1 << TWINT) | (1 << TWEN);
if (!waitInt()) return 3;
twst = TWSR & 0xF8;
} while (twst == TW_MT_SLA_NACK && retry-- > 0);
if (twst != TW_MT_SLA_ACK) return 4;
+ //Serial.print(address, HEX);
+ //Serial.print(" ");
TWDR = address; // send data to the previously addressed device
TWCR = (1 << TWINT) | (1 << TWEN);
if (!waitInt()) return 5;
twst = TWSR & 0xF8;
if (twst != TW_MT_DATA_ACK) return 6;
- TWDR = value; // send data to the previously addressed device
+ for (byte i = 0; i < num; i++) {
+ //Serial.print(data[i], HEX);
+ //Serial.print(" ");
+ TWDR = data[i]; // send data to the previously addressed device
+ TWCR = (1 << TWINT) | (1 << TWEN);
+ if (!waitInt()) return 7;
+ twst = TWSR & 0xF8;
+ if (twst != TW_MT_DATA_ACK) return 8;
+ }
+ //Serial.print("\n");
+
+ return 0;
+ }
+
+ byte Fastwire::write(byte value) {
+ byte twst;
+ //Serial.println(value, HEX);
+ TWDR = value; // send data
TWCR = (1 << TWINT) | (1 << TWEN);
- if (!waitInt()) return 7;
+ if (!waitInt()) return 1;
twst = TWSR & 0xF8;
- if (twst != TW_MT_DATA_ACK) return 8;
-
+ if (twst != TW_MT_DATA_ACK) return 2;
return 0;
}
@@ -789,13 +827,17 @@ uint16_t I2Cdev::readTimeout = I2CDEV_DEFAULT_READ_TIMEOUT;
twst = TWSR & 0xF8;
if (twst != TW_START && twst != TW_REP_START) return 17;
+ //Serial.print(device, HEX);
+ //Serial.print(" ");
TWDR = device & 0xfe; // send device address to write
TWCR = (1 << TWINT) | (1 << TWEN);
if (!waitInt()) return 18;
twst = TWSR & 0xF8;
} while (twst == TW_MT_SLA_NACK && retry-- > 0);
if (twst != TW_MT_SLA_ACK) return 19;
+ //Serial.print(address, HEX);
+ //Serial.print(" ");
TWDR = address; // send data to the previously addressed device
TWCR = (1 << TWINT) | (1 << TWEN);
if (!waitInt()) return 20;
@@ -811,26 +853,42 @@ uint16_t I2Cdev::readTimeout = I2CDEV_DEFAULT_READ_TIMEOUT;
twst = TWSR & 0xF8;
if (twst != TW_START && twst != TW_REP_START) return 23;
+ //Serial.print(device, HEX);
+ //Serial.print(" ");
TWDR = device | 0x01; // send device address with the read bit (1)
TWCR = (1 << TWINT) | (1 << TWEN);
if (!waitInt()) return 24;
twst = TWSR & 0xF8;
} while (twst == TW_MR_SLA_NACK && retry-- > 0);
if (twst != TW_MR_SLA_ACK) return 25;
- for(uint8_t i = 0; i < num; i++) {
+ for (uint8_t i = 0; i < num; i++) {
if (i == num - 1)
- TWCR = (1 << TWINT) | (1 << TWEN);
+ TWCR = (1 << TWINT) | (1 << TWEN);
else
TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWEA);
if (!waitInt()) return 26;
twst = TWSR & 0xF8;
if (twst != TW_MR_DATA_ACK && twst != TW_MR_DATA_NACK) return twst;
data[i] = TWDR;
+ //Serial.print(data[i], HEX);
+ //Serial.print(" ");
}
+ //Serial.print("\n");
+ stop();
+
+ return 0;
+ }
+ void Fastwire::reset() {
+ TWCR = 0;
+ }
+
+ byte Fastwire::stop() {
+ TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWSTO);
+ if (!waitInt()) return 1;
return 0;
- }
+ }
#endif
#if I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE
@@ -940,7 +998,7 @@ uint16_t I2Cdev::readTimeout = I2CDEV_DEFAULT_READ_TIMEOUT;
twi_Write_Vars *ptwv = 0;
static void (*fNextInterruptFunction)(void) = 0;
-
+
void twi_Finish(byte bRetVal) {
if (ptwv) {
free(ptwv);
@@ -1297,7 +1355,7 @@ uint16_t I2Cdev::readTimeout = I2CDEV_DEFAULT_READ_TIMEOUT;
txBufferIndex = 0;
txBufferLength = 0;
}
-
+
uint8_t TwoWire::endTransmission(uint16_t timeout) {
// transmit buffer (blocking)
//int8_t ret =
View
61 Arduino/I2Cdev/I2Cdev.h
@@ -3,24 +3,25 @@
// 6/9/2012 by Jeff Rowberg <jeff@rowberg.net>
//
// Changelog:
-// 2013-05-05 - fix issue with writing bit values to words (Sasquatch/Farzanegan)
-// 2012-06-09 - fix major issue with reading > 32 bytes at a time with Arduino Wire
-// - add compiler warnings when using outdated or IDE or limited I2Cdev implementation
-// 2011-11-01 - fix write*Bits mask calculation (thanks sasquatch @ Arduino forums)
-// 2011-10-03 - added automatic Arduino version detection for ease of use
-// 2011-10-02 - added Gene Knight's NBWire TwoWire class implementation with small modifications
-// 2011-08-31 - added support for Arduino 1.0 Wire library (methods are different from 0.x)
-// 2011-08-03 - added optional timeout parameter to read* methods to easily change from default
-// 2011-08-02 - added support for 16-bit registers
-// - fixed incorrect Doxygen comments on some methods
-// - added timeout value for read operations (thanks mem @ Arduino forums)
-// 2011-07-30 - changed read/write function structures to return success or byte counts
-// - made all methods static for multi-device memory savings
-// 2011-07-28 - initial release
+// 2013-05-06 - add Francesco Ferrara's Fastwire v0.24 implementation with small modifications
+// 2013-05-05 - fix issue with writing bit values to words (Sasquatch/Farzanegan)
+// 2012-06-09 - fix major issue with reading > 32 bytes at a time with Arduino Wire
+// - add compiler warnings when using outdated or IDE or limited I2Cdev implementation
+// 2011-11-01 - fix write*Bits mask calculation (thanks sasquatch @ Arduino forums)
+// 2011-10-03 - added automatic Arduino version detection for ease of use
+// 2011-10-02 - added Gene Knight's NBWire TwoWire class implementation with small modifications
+// 2011-08-31 - added support for Arduino 1.0 Wire library (methods are different from 0.x)
+// 2011-08-03 - added optional timeout parameter to read* methods to easily change from default
+// 2011-08-02 - added support for 16-bit registers
+// - fixed incorrect Doxygen comments on some methods
+// - added timeout value for read operations (thanks mem @ Arduino forums)
+// 2011-07-30 - changed read/write function structures to return success or byte counts
+// - made all methods static for multi-device memory savings
+// 2011-07-28 - initial release
/* ============================================
I2Cdev device library code is placed under the MIT license
-Copyright (c) 2012 Jeff Rowberg
+Copyright (c) 2013 Jeff Rowberg
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
@@ -45,6 +46,12 @@ THE SOFTWARE.
#ifndef _I2CDEV_H_
#define _I2CDEV_H_
+// -----------------------------------------------------------------------------
+// I2C interface implementation setting
+// -----------------------------------------------------------------------------
+#define I2CDEV_IMPLEMENTATION I2CDEV_ARDUINO_WIRE
+//#define I2CDEV_IMPLEMENTATION I2CDEV_BUILTIN_FASTWIRE
+
// comment this out if you are using a non-optimal IDE/implementation setting
// but want the compiler to shut up about it
#define I2CDEV_IMPLEMENTATION_WARNINGS
@@ -56,14 +63,6 @@ THE SOFTWARE.
#define I2CDEV_BUILTIN_NBWIRE 2 // Tweaked Wire object from Gene Knight's NBWire project
// ^^^ NBWire implementation is still buggy w/some interrupts!
#define I2CDEV_BUILTIN_FASTWIRE 3 // FastWire object from Francesco Ferrara's project
- // ^^^ FastWire implementation in I2Cdev is INCOMPLETE!
-#define I2CDEV_I2CMASTER_LIBRARY 4 // I2C object from DSSCircuits I2C-Master Library at
- // https://github.com/DSSCircuits/I2C-Master-Library
-
-// -----------------------------------------------------------------------------
-// I2C interface implementation setting
-// -----------------------------------------------------------------------------
-#define I2CDEV_IMPLEMENTATION I2CDEV_ARDUINO_WIRE
// -----------------------------------------------------------------------------
// Arduino-style "Serial.print" debug constant (uncomment to enable)
@@ -78,13 +77,7 @@ THE SOFTWARE.
#endif
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include <Wire.h>
- #else
- #if I2CDEV_IMPLEMENTATION == I2CDEV_I2CMASTER_LIBRARY
- #include <I2C.h>
- #endif
#endif
-#else
- #include "ArduinoWrapper.h"
#endif
// 1000ms default read timeout (modify with "I2Cdev::readTimeout = [ms];")
@@ -117,9 +110,9 @@ class I2Cdev {
#if I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
//////////////////////
- // FastWire 0.2
+ // FastWire 0.24
// This is a library to help faster programs to read I2C devices.
- // Copyright(C) 2011
+ // Copyright(C) 2012
// Francesco Ferrara
//////////////////////
@@ -150,8 +143,12 @@ class I2Cdev {
public:
static void setup(int khz, boolean pullup);
- static byte write(byte device, byte address, byte value);
+ static byte beginTransmission(byte device);
+ static byte write(byte value);
+ static byte writeBuf(byte device, byte address, byte *data, byte num);
static byte readBuf(byte device, byte address, byte *data, byte num);
+ static void reset();
+ static byte stop();
};
#endif
View
54 Arduino/MPU6050/Examples/MPU6050_DMP6/MPU6050_DMP6.ino
@@ -3,17 +3,19 @@
// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
//
// Changelog:
-// 2012-06-21 - added note about Arduino 1.0.1 + Leonardo compatibility error
-// 2012-06-20 - improved FIFO overflow handling and simplified read process
-// 2012-06-19 - completely rearranged DMP initialization code and simplification
-// 2012-06-13 - pull gyro and accel data from FIFO packet instead of reading directly
-// 2012-06-09 - fix broken FIFO read sequence and change interrupt detection to RISING
-// 2012-06-05 - add gravity-compensated initial reference frame acceleration output
-// - add 3D math helper file to DMP6 example sketch
-// - add Euler output and Yaw/Pitch/Roll output formats
-// 2012-06-04 - remove accel offset clearing for better results (thanks Sungon Lee)
-// 2012-06-01 - fixed gyro sensitivity to be 2000 deg/sec instead of 250
-// 2012-05-30 - basic DMP initialization working
+// 2013-05-08 - added seamless Fastwire support
+// - added note about gyro calibration
+// 2012-06-21 - added note about Arduino 1.0.1 + Leonardo compatibility error
+// 2012-06-20 - improved FIFO overflow handling and simplified read process
+// 2012-06-19 - completely rearranged DMP initialization code and simplification
+// 2012-06-13 - pull gyro and accel data from FIFO packet instead of reading directly
+// 2012-06-09 - fix broken FIFO read sequence and change interrupt detection to RISING
+// 2012-06-05 - add gravity-compensated initial reference frame acceleration output
+// - add 3D math helper file to DMP6 example sketch
+// - add Euler output and Yaw/Pitch/Roll output formats
+// 2012-06-04 - remove accel offset clearing for better results (thanks Sungon Lee)
+// 2012-06-01 - fixed gyro sensitivity to be 2000 deg/sec instead of 250
+// 2012-05-30 - basic DMP initialization working
/* ============================================
I2Cdev device library code is placed under the MIT license
@@ -39,22 +41,25 @@ THE SOFTWARE.
===============================================
*/
-// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
-// is used in I2Cdev.h
-#include "Wire.h"
-
// I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files
// for both classes must be in the include path of your project
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
//#include "MPU6050.h" // not necessary if using MotionApps include file
+// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
+// is used in I2Cdev.h
+#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
+ #include "Wire.h"
+#endif
+
// class default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board)
// AD0 high = 0x69
MPU6050 mpu;
+//MPU6050 mpu(0x69); // <-- use for AD0 high
/* =========================================================================
NOTE: In addition to connection 3.3v, GND, SDA, and SCL, this sketch
@@ -92,7 +97,7 @@ MPU6050 mpu;
// from the FIFO. Note this also requires gravity vector calculations.
// Also note that yaw/pitch/roll angles suffer from gimbal lock (for
// more info, see: http://en.wikipedia.org/wiki/Gimbal_lock)
-//#define OUTPUT_READABLE_YAWPITCHROLL
+#define OUTPUT_READABLE_YAWPITCHROLL
// uncomment "OUTPUT_READABLE_REALACCEL" if you want to see acceleration
// components with gravity removed. This acceleration reference frame is
@@ -109,7 +114,7 @@ MPU6050 mpu;
// uncomment "OUTPUT_TEAPOT" if you want output that matches the
// format used for the InvenSense teapot demo
-#define OUTPUT_TEAPOT
+//#define OUTPUT_TEAPOT
@@ -155,7 +160,12 @@ void dmpDataReady() {
void setup() {
// join I2C bus (I2Cdev library doesn't do this automatically)
- Wire.begin();
+ #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
+ Wire.begin();
+ TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz)
+ #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
+ Fastwire::setup(400, true);
+ #endif
// initialize serial communication
// (115200 chosen because it is required for Teapot Demo output, but it's
@@ -186,7 +196,13 @@ void setup() {
// load and configure the DMP
Serial.println(F("Initializing DMP..."));
devStatus = mpu.dmpInitialize();
-
+
+ // supply your own gyro offsets here, scaled for min sensitivity
+ mpu.setXGyroOffset(220);
+ mpu.setYGyroOffset(76);
+ mpu.setZGyroOffset(-85);
+ mpu.setZAccelOffset(1788); // 1688 factory default for my test chip
+
// make sure it worked (returns 0 if so)
if (devStatus == 0) {
// turn on the DMP, now that it's ready
View
85 Arduino/MPU6050/Examples/MPU6050_raw/MPU6050_raw.ino
@@ -3,7 +3,9 @@
// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
//
// Changelog:
-// 2011-10-07 - initial release
+// 2013-05-08 - added multiple output formats
+// - added seamless Fastwire support
+// 2011-10-07 - initial release
/* ============================================
I2Cdev device library code is placed under the MIT license
@@ -29,30 +31,51 @@ THE SOFTWARE.
===============================================
*/
-// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
-// is used in I2Cdev.h
-#include "Wire.h"
-
// I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files
// for both classes must be in the include path of your project
#include "I2Cdev.h"
#include "MPU6050.h"
+// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
+// is used in I2Cdev.h
+#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
+ #include "Wire.h"
+#endif
+
// class default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for InvenSense evaluation board)
// AD0 high = 0x69
MPU6050 accelgyro;
+//MPU6050 accelgyro(0x69); // <-- use for AD0 high
int16_t ax, ay, az;
int16_t gx, gy, gz;
+
+
+// uncomment "OUTPUT_READABLE_ACCELGYRO" if you want to see a tab-separated
+// list of the accel X/Y/Z and then gyro X/Y/Z values in decimal. Easy to read,
+// not so easy to parse, and slow(er) over UART.
+#define OUTPUT_READABLE_ACCELGYRO
+
+// uncomment "OUTPUT_BINARY_ACCELGYRO" to send all 6 axes of data as 16-bit
+// binary, one right after the other. This is very fast (as fast as possible
+// without compression or data loss), and easy to parse, but impossible to read
+// for a human.
+//#define OUTPUT_BINARY_ACCELGYRO
+
+
#define LED_PIN 13
bool blinkState = false;
void setup() {
// join I2C bus (I2Cdev library doesn't do this automatically)
- Wire.begin();
+ #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
+ Wire.begin();
+ #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
+ Fastwire::setup(400, true);
+ #endif
// initialize serial communication
// (38400 chosen because it works as well at 8MHz as it does at 16MHz, but
@@ -67,6 +90,29 @@ void setup() {
Serial.println("Testing device connections...");
Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
+ // use the code below to change accel/gyro offset values
+ /*
+ Serial.println("Updating internal sensor offsets...");
+ // -76 -2359 1688 0 0 0
+ Serial.print(accelgyro.getXAccelOffset()); Serial.print("\t"); // -76
+ Serial.print(accelgyro.getYAccelOffset()); Serial.print("\t"); // -2359
+ Serial.print(accelgyro.getZAccelOffset()); Serial.print("\t"); // 1688
+ Serial.print(accelgyro.getXGyroOffset()); Serial.print("\t"); // 0
+ Serial.print(accelgyro.getYGyroOffset()); Serial.print("\t"); // 0
+ Serial.print(accelgyro.getZGyroOffset()); Serial.print("\t"); // 0
+ Serial.print("\n");
+ accelgyro.setXGyroOffset(220);
+ accelgyro.setYGyroOffset(76);
+ accelgyro.setZGyroOffset(-85);
+ Serial.print(accelgyro.getXAccelOffset()); Serial.print("\t"); // -76
+ Serial.print(accelgyro.getYAccelOffset()); Serial.print("\t"); // -2359
+ Serial.print(accelgyro.getZAccelOffset()); Serial.print("\t"); // 1688
+ Serial.print(accelgyro.getXGyroOffset()); Serial.print("\t"); // 0
+ Serial.print(accelgyro.getYGyroOffset()); Serial.print("\t"); // 0
+ Serial.print(accelgyro.getZGyroOffset()); Serial.print("\t"); // 0
+ Serial.print("\n");
+ */
+
// configure Arduino LED for
pinMode(LED_PIN, OUTPUT);
}
@@ -79,14 +125,25 @@ void loop() {
//accelgyro.getAcceleration(&ax, &ay, &az);
//accelgyro.getRotation(&gx, &gy, &gz);
- // display tab-separated accel/gyro x/y/z values
- Serial.print("a/g:\t");
- Serial.print(ax); Serial.print("\t");
- Serial.print(ay); Serial.print("\t");
- Serial.print(az); Serial.print("\t");
- Serial.print(gx); Serial.print("\t");
- Serial.print(gy); Serial.print("\t");
- Serial.println(gz);
+ #ifdef OUTPUT_READABLE_ACCELGYRO
+ // display tab-separated accel/gyro x/y/z values
+ Serial.print("a/g:\t");
+ Serial.print(ax); Serial.print("\t");
+ Serial.print(ay); Serial.print("\t");
+ Serial.print(az); Serial.print("\t");
+ Serial.print(gx); Serial.print("\t");
+ Serial.print(gy); Serial.print("\t");
+ Serial.println(gz);
+ #endif
+
+ #ifdef OUTPUT_BINARY_ACCELGYRO
+ Serial.write((uint8_t)(ax >> 8)); Serial.write((uint8_t)(ax & 0xFF));
+ Serial.write((uint8_t)(ay >> 8)); Serial.write((uint8_t)(ay & 0xFF));
+ Serial.write((uint8_t)(az >> 8)); Serial.write((uint8_t)(az & 0xFF));
+ Serial.write((uint8_t)(gx >> 8)); Serial.write((uint8_t)(gx & 0xFF));
+ Serial.write((uint8_t)(gy >> 8)); Serial.write((uint8_t)(gy & 0xFF));
+ Serial.write((uint8_t)(gz >> 8)); Serial.write((uint8_t)(gz & 0xFF));
+ #endif
// blink LED to indicate activity
blinkState = !blinkState;
View
2 Arduino/MPU6050/MPU6050.h
@@ -38,7 +38,7 @@ THE SOFTWARE.
#define _MPU6050_H_
#include "I2Cdev.h"
-#include <avr/pgmspace.h>
+//#include <avr/pgmspace.h>
View
46 Arduino/MPU6050/MPU6050_6Axis_MotionApps20.h
@@ -40,7 +40,51 @@ THE SOFTWARE.
#define MPU6050_INCLUDE_DMP_MOTIONAPPS20
#include "MPU6050.h"
-#include <avr/pgmspace.h>
+
+// Tom Carpenter's conditional PROGMEM code
+// http://forum.arduino.cc/index.php?topic=129407.0
+#ifndef __arm__
+ #include <avr/pgmspace.h>
+#else
+ // Teensy 3.0 library conditional PROGMEM code from Paul Stoffregen
+ #ifndef __PGMSPACE_H_
+ #define __PGMSPACE_H_ 1
+ #include <inttypes.h>
+
+ #define PROGMEM
+ #define PGM_P const char *
+ #define PSTR(str) (str)
+ #define F(x) x
+
+ typedef void prog_void;
+ typedef char prog_char;
+ typedef unsigned char prog_uchar;
+ typedef int8_t prog_int8_t;
+ typedef uint8_t prog_uint8_t;
+ typedef int16_t prog_int16_t;
+ typedef uint16_t prog_uint16_t;
+ typedef int32_t prog_int32_t;
+ typedef uint32_t prog_uint32_t;
+
+ #define strcpy_P(dest, src) strcpy((dest), (src))
+ #define strcat_P(dest, src) strcat((dest), (src))
+ #define strcmp_P(a, b) strcmp((a), (b))
+
+ #define pgm_read_byte(addr) (*(const unsigned char *)(addr))
+ #define pgm_read_word(addr) (*(const unsigned short *)(addr))
+ #define pgm_read_dword(addr) (*(const unsigned long *)(addr))
+ #define pgm_read_float(addr) (*(const float *)(addr))
+
+ #define pgm_read_byte_near(addr) pgm_read_byte(addr)
+ #define pgm_read_word_near(addr) pgm_read_word(addr)
+ #define pgm_read_dword_near(addr) pgm_read_dword(addr)
+ #define pgm_read_float_near(addr) pgm_read_float(addr)
+ #define pgm_read_byte_far(addr) pgm_read_byte(addr)
+ #define pgm_read_word_far(addr) pgm_read_word(addr)
+ #define pgm_read_dword_far(addr) pgm_read_dword(addr)
+ #define pgm_read_float_far(addr) pgm_read_float(addr)
+ #endif
+#endif
/* Source is from the InvenSense MotionApps v2 demo code. Original source is
* unavailable, unless you happen to be amazing as decompiling binary by
View
46 Arduino/MPU6050/MPU6050_9Axis_MotionApps41.h
@@ -40,7 +40,51 @@ THE SOFTWARE.
#define MPU6050_INCLUDE_DMP_MOTIONAPPS41
#include "MPU6050.h"
-#include <avr/pgmspace.h>
+
+// Tom Carpenter's conditional PROGMEM code
+// http://forum.arduino.cc/index.php?topic=129407.0
+#ifndef __arm__
+ #include <avr/pgmspace.h>
+#else
+ // Teensy 3.0 library conditional PROGMEM code from Paul Stoffregen
+ #ifndef __PGMSPACE_H_
+ #define __PGMSPACE_H_ 1
+ #include <inttypes.h>
+
+ #define PROGMEM
+ #define PGM_P const char *
+ #define PSTR(str) (str)
+ #define F(x) x
+
+ typedef void prog_void;
+ typedef char prog_char;
+ typedef unsigned char prog_uchar;
+ typedef int8_t prog_int8_t;
+ typedef uint8_t prog_uint8_t;
+ typedef int16_t prog_int16_t;
+ typedef uint16_t prog_uint16_t;
+ typedef int32_t prog_int32_t;
+ typedef uint32_t prog_uint32_t;
+
+ #define strcpy_P(dest, src) strcpy((dest), (src))
+ #define strcat_P(dest, src) strcat((dest), (src))
+ #define strcmp_P(a, b) strcmp((a), (b))
+
+ #define pgm_read_byte(addr) (*(const unsigned char *)(addr))
+ #define pgm_read_word(addr) (*(const unsigned short *)(addr))
+ #define pgm_read_dword(addr) (*(const unsigned long *)(addr))
+ #define pgm_read_float(addr) (*(const float *)(addr))
+
+ #define pgm_read_byte_near(addr) pgm_read_byte(addr)
+ #define pgm_read_word_near(addr) pgm_read_word(addr)
+ #define pgm_read_dword_near(addr) pgm_read_dword(addr)
+ #define pgm_read_float_near(addr) pgm_read_float(addr)
+ #define pgm_read_byte_far(addr) pgm_read_byte(addr)
+ #define pgm_read_word_far(addr) pgm_read_word(addr)
+ #define pgm_read_dword_far(addr) pgm_read_dword(addr)
+ #define pgm_read_float_far(addr) pgm_read_float(addr)
+ #endif
+#endif
// NOTE! Enabling DEBUG adds about 3.3kB to the flash program size.
// Debug output is now working even on ATMega328P MCUs (e.g. Arduino Uno)

0 comments on commit fc92547

Please sign in to comment.
Something went wrong with that request. Please try again.