Brewery/gravitymon
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Swiched to own forks of libs for better version control

Browse Source
This commit is contained in:
Magnus Persson 2022-01-01 17:42:50 +01:00
parent 340356350b
commit 29174bf1f9
6 changed files with 1014 additions and 929 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

264
lib/mpu6050/I2Cdev.cpp
View File

@ -3,6 +3,9 @@
// 2013-06-05 by Jeff Rowberg <jeff@rowberg.net>
//
// Changelog:
// 2021-09-28 - allow custom Wire object as transaction function argument
// 2020-01-20 - hardija : complete support for Teensy 3.x
// 2015-10-30 - simondlevy : support i2c_t3 for Teensy3.1
// 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
@ -87,11 +90,6 @@ THE SOFTWARE.
#endif
#ifndef BUFFER_LENGTH
// band-aid fix for platforms without Wire-defined BUFFER_LENGTH (removed from some official implementations)
#define BUFFER_LENGTH 32
#endif
/** Default constructor.
*/
I2Cdev::I2Cdev() {
@ -105,9 +103,9 @@ I2Cdev::I2Cdev() {
* @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
* @return Status of read operation (true = success)
*/
int8_t I2Cdev::readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data, uint16_t timeout) {
int8_t I2Cdev::readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data, uint16_t timeout, void *wireObj) {
uint8_t b;
uint8_t count = readByte(devAddr, regAddr, &b, timeout);
uint8_t count = readByte(devAddr, regAddr, &b, timeout, wireObj);
*data = b & (1 << bitNum);
return count;
}
@ -120,9 +118,9 @@ int8_t I2Cdev::readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t
* @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
* @return Status of read operation (true = success)
*/
int8_t I2Cdev::readBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t *data, uint16_t timeout) {
int8_t I2Cdev::readBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t *data, uint16_t timeout, void *wireObj) {
uint16_t b;
uint8_t count = readWord(devAddr, regAddr, &b, timeout);
uint8_t count = readWord(devAddr, regAddr, &b, timeout, wireObj);
*data = b & (1 << bitNum);
return count;
}
@ -136,14 +134,14 @@ int8_t I2Cdev::readBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16
* @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
* @return Status of read operation (true = success)
*/
int8_t I2Cdev::readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data, uint16_t timeout) {
int8_t I2Cdev::readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data, uint16_t timeout, void *wireObj) {
// 01101001 read byte
// 76543210 bit numbers
// xxx args: bitStart=4, length=3
// 010 masked
// -> 010 shifted
uint8_t count, b;
if ((count = readByte(devAddr, regAddr, &b, timeout)) != 0) {
if ((count = readByte(devAddr, regAddr, &b, timeout, wireObj)) != 0) {
uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1);
b &= mask;
b >>= (bitStart - length + 1);
@ -161,7 +159,7 @@ int8_t I2Cdev::readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint
* @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
* @return Status of read operation (1 = success, 0 = failure, -1 = timeout)
*/
int8_t I2Cdev::readBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t *data, uint16_t timeout) {
int8_t I2Cdev::readBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t *data, uint16_t timeout, void *wireObj) {
// 1101011001101001 read byte
// fedcba9876543210 bit numbers
// xxx args: bitStart=12, length=3
@ -169,7 +167,7 @@ int8_t I2Cdev::readBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uin
// -> 010 shifted
uint8_t count;
uint16_t w;
if ((count = readWord(devAddr, regAddr, &w, timeout)) != 0) {
if ((count = readWord(devAddr, regAddr, &w, timeout, wireObj)) != 0) {
uint16_t mask = ((1 << length) - 1) << (bitStart - length + 1);
w &= mask;
w >>= (bitStart - length + 1);
@ -185,8 +183,8 @@ int8_t I2Cdev::readBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uin
* @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
* @return Status of read operation (true = success)
*/
int8_t I2Cdev::readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data, uint16_t timeout) {
return readBytes(devAddr, regAddr, 1, data, timeout);
int8_t I2Cdev::readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data, uint16_t timeout, void *wireObj) {
return readBytes(devAddr, regAddr, 1, data, timeout, wireObj);
}
/** Read single word from a 16-bit device register.
@ -196,8 +194,8 @@ int8_t I2Cdev::readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data, uint16_
* @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
* @return Status of read operation (true = success)
*/
int8_t I2Cdev::readWord(uint8_t devAddr, uint8_t regAddr, uint16_t *data, uint16_t timeout) {
return readWords(devAddr, regAddr, 1, data, timeout);
int8_t I2Cdev::readWord(uint8_t devAddr, uint8_t regAddr, uint16_t *data, uint16_t timeout, void *wireObj) {
return readWords(devAddr, regAddr, 1, data, timeout, wireObj);
}
/** Read multiple bytes from an 8-bit device register.
@ -208,7 +206,7 @@ int8_t I2Cdev::readWord(uint8_t devAddr, uint8_t regAddr, uint16_t *data, uint16
* @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
* @return Number of bytes read (-1 indicates failure)
*/
int8_t I2Cdev::readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, uint16_t timeout) {
int8_t I2Cdev::readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, uint16_t timeout, void *wireObj) {
#ifdef I2CDEV_SERIAL_DEBUG
Serial.print("I2C (0x");
Serial.print(devAddr, HEX);
@ -223,70 +221,72 @@ int8_t I2Cdev::readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8
uint32_t t1 = millis();
#if (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE || I2CDEV_IMPLEMENTATION == I2CDEV_TEENSY_3X_WIRE)
TwoWire *useWire = &Wire;
if (wireObj) useWire = (TwoWire *)wireObj;
#if (ARDUINO < 100)
// Arduino v00xx (before v1.0), Wire library
// I2C/TWI subsystem uses internal buffer that breaks with large data requests
// so if user requests more than BUFFER_LENGTH bytes, we have to do it in
// so if user requests more than I2CDEVLIB_WIRE_BUFFER_LENGTH bytes, we have to do it in
// smaller chunks instead of all at once
for (uint8_t k = 0; k < length; k += min((int)length, BUFFER_LENGTH)) {
Wire.beginTransmission(devAddr);
Wire.send(regAddr);
Wire.endTransmission();
Wire.beginTransmission(devAddr);
Wire.requestFrom(devAddr, (uint8_t)min(length - k, BUFFER_LENGTH));
for (int k = 0; k < length; k += min((int)length, I2CDEVLIB_WIRE_BUFFER_LENGTH)) {
useWire->beginTransmission(devAddr);
useWire->send(regAddr);
useWire->endTransmission();
useWire->beginTransmission(devAddr);
useWire->requestFrom((uint8_t)devAddr, (uint8_t)min((int)length - k, I2CDEVLIB_WIRE_BUFFER_LENGTH));
for (; Wire.available() && (timeout == 0 || millis() - t1 < timeout); count++) {
data[count] = Wire.receive();
for (; useWire->available() && (timeout == 0 || millis() - t1 < timeout); count++) {
data[count] = useWire->receive();
#ifdef I2CDEV_SERIAL_DEBUG
Serial.print(data[count], HEX);
if (count + 1 < length) Serial.print(" ");
#endif
}
Wire.endTransmission();
useWire->endTransmission();
}
#elif (ARDUINO == 100)
// Arduino v1.0.0, Wire library
// Adds standardized write() and read() stream methods instead of send() and receive()
// I2C/TWI subsystem uses internal buffer that breaks with large data requests
// so if user requests more than BUFFER_LENGTH bytes, we have to do it in
// so if user requests more than I2CDEVLIB_WIRE_BUFFER_LENGTH bytes, we have to do it in
// smaller chunks instead of all at once
for (uint8_t k = 0; k < length; k += min((int)length, BUFFER_LENGTH)) {
Wire.beginTransmission(devAddr);
Wire.write(regAddr);
Wire.endTransmission();
Wire.beginTransmission(devAddr);
Wire.requestFrom(devAddr, (uint8_t)min(length - k, BUFFER_LENGTH));
for (int k = 0; k < length; k += min((int)length, I2CDEVLIB_WIRE_BUFFER_LENGTH)) {
useWire->beginTransmission(devAddr);
useWire->write(regAddr);
useWire->endTransmission();
useWire->beginTransmission(devAddr);
useWire->requestFrom((uint8_t)devAddr, (uint8_t)min((int)length - k, I2CDEVLIB_WIRE_BUFFER_LENGTH));
for (; Wire.available() && (timeout == 0 || millis() - t1 < timeout); count++) {
data[count] = Wire.read();
for (; useWire->available() && (timeout == 0 || millis() - t1 < timeout); count++) {
data[count] = useWire->read();
#ifdef I2CDEV_SERIAL_DEBUG
Serial.print(data[count], HEX);
if (count + 1 < length) Serial.print(" ");
#endif
}
Wire.endTransmission();
useWire->endTransmission();
}
#elif (ARDUINO > 100)
// Arduino v1.0.1+, Wire library
// Adds official support for repeated start condition, yay!
// I2C/TWI subsystem uses internal buffer that breaks with large data requests
// so if user requests more than BUFFER_LENGTH bytes, we have to do it in
// so if user requests more than I2CDEVLIB_WIRE_BUFFER_LENGTH bytes, we have to do it in
// smaller chunks instead of all at once
for (uint8_t k = 0; k < length; k += min((int)length, BUFFER_LENGTH)) {
Wire.beginTransmission(devAddr);
Wire.write(regAddr);
Wire.endTransmission();
Wire.beginTransmission(devAddr);
Wire.requestFrom(devAddr, (uint8_t)min(length - k, BUFFER_LENGTH));
for (int k = 0; k < length; k += min((int)length, I2CDEVLIB_WIRE_BUFFER_LENGTH)) {
useWire->beginTransmission(devAddr);
useWire->write(regAddr);
useWire->endTransmission();
useWire->beginTransmission(devAddr);
useWire->requestFrom((uint8_t)devAddr, (uint8_t)min((int)length - k, I2CDEVLIB_WIRE_BUFFER_LENGTH));
for (; Wire.available() && (timeout == 0 || millis() - t1 < timeout); count++) {
data[count] = Wire.read();
for (; useWire->available() && (timeout == 0 || millis() - t1 < timeout); count++) {
data[count] = useWire->read();
#ifdef I2CDEV_SERIAL_DEBUG
Serial.print(data[count], HEX);
if (count + 1 < length) Serial.print(" ");
@ -328,7 +328,7 @@ int8_t I2Cdev::readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8
* @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 (-1 indicates failure)
*/
int8_t I2Cdev::readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data, uint16_t timeout) {
int8_t I2Cdev::readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data, uint16_t timeout, void *wireObj) {
#ifdef I2CDEV_SERIAL_DEBUG
Serial.print("I2C (0x");
Serial.print(devAddr, HEX);
@ -343,28 +343,30 @@ int8_t I2Cdev::readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint1
uint32_t t1 = millis();
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE || I2CDEV_IMPLEMENTATION == I2CDEV_TEENSY_3X_WIRE
TwoWire *useWire = &Wire;
if (wireObj) useWire = (TwoWire *)wireObj;
#if (ARDUINO < 100)
// Arduino v00xx (before v1.0), Wire library
// I2C/TWI subsystem uses internal buffer that breaks with large data requests
// so if user requests more than BUFFER_LENGTH bytes, we have to do it in
// so if user requests more than I2CDEVLIB_WIRE_BUFFER_LENGTH bytes, we have to do it in
// smaller chunks instead of all at once
for (uint8_t k = 0; k < length * 2; k += min(length * 2, BUFFER_LENGTH)) {
Wire.beginTransmission(devAddr);
Wire.send(regAddr);
Wire.endTransmission();
Wire.beginTransmission(devAddr);
Wire.requestFrom(devAddr, (uint8_t)(length * 2)); // length=words, this wants bytes
for (uint8_t k = 0; k < length * 2; k += min(length * 2, I2CDEVLIB_WIRE_BUFFER_LENGTH)) {
useWire->beginTransmission(devAddr);
useWire->send(regAddr);
useWire->endTransmission();
useWire->beginTransmission(devAddr);
useWire->requestFrom(devAddr, (uint8_t)(length * 2)); // length=words, this wants bytes
bool msb = true; // starts with MSB, then LSB
for (; Wire.available() && count < length && (timeout == 0 || millis() - t1 < timeout);) {
for (; useWire->available() && count < length && (timeout == 0 || millis() - t1 < timeout);) {
if (msb) {
// first byte is bits 15-8 (MSb=15)
data[count] = Wire.receive() << 8;
data[count] = useWire->receive() << 8;
} else {
// second byte is bits 7-0 (LSb=0)
data[count] |= Wire.receive();
data[count] |= useWire->receive();
#ifdef I2CDEV_SERIAL_DEBUG
Serial.print(data[count], HEX);
if (count + 1 < length) Serial.print(" ");
@ -374,30 +376,30 @@ int8_t I2Cdev::readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint1
msb = !msb;
}
Wire.endTransmission();
useWire->endTransmission();
}
#elif (ARDUINO == 100)
// Arduino v1.0.0, Wire library
// Adds standardized write() and read() stream methods instead of send() and receive()
// I2C/TWI subsystem uses internal buffer that breaks with large data requests
// so if user requests more than BUFFER_LENGTH bytes, we have to do it in
// so if user requests more than I2CDEVLIB_WIRE_BUFFER_LENGTH bytes, we have to do it in
// smaller chunks instead of all at once
for (uint8_t k = 0; k < length * 2; k += min(length * 2, BUFFER_LENGTH)) {
Wire.beginTransmission(devAddr);
Wire.write(regAddr);
Wire.endTransmission();
Wire.beginTransmission(devAddr);
Wire.requestFrom(devAddr, (uint8_t)(length * 2)); // length=words, this wants bytes
for (uint8_t k = 0; k < length * 2; k += min(length * 2, I2CDEVLIB_WIRE_BUFFER_LENGTH)) {
useWire->beginTransmission(devAddr);
useWire->write(regAddr);
useWire->endTransmission();
useWire->beginTransmission(devAddr);
useWire->requestFrom(devAddr, (uint8_t)(length * 2)); // length=words, this wants bytes
bool msb = true; // starts with MSB, then LSB
for (; Wire.available() && count < length && (timeout == 0 || millis() - t1 < timeout);) {
for (; useWire->available() && count < length && (timeout == 0 || millis() - t1 < timeout);) {
if (msb) {
// first byte is bits 15-8 (MSb=15)
data[count] = Wire.read() << 8;
data[count] = useWire->read() << 8;
} else {
// second byte is bits 7-0 (LSb=0)
data[count] |= Wire.read();
data[count] |= useWire->read();
#ifdef I2CDEV_SERIAL_DEBUG
Serial.print(data[count], HEX);
if (count + 1 < length) Serial.print(" ");
@ -407,30 +409,30 @@ int8_t I2Cdev::readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint1
msb = !msb;
}
Wire.endTransmission();
useWire->endTransmission();
}
#elif (ARDUINO > 100)
// Arduino v1.0.1+, Wire library
// Adds official support for repeated start condition, yay!
// I2C/TWI subsystem uses internal buffer that breaks with large data requests
// so if user requests more than BUFFER_LENGTH bytes, we have to do it in
// so if user requests more than I2CDEVLIB_WIRE_BUFFER_LENGTH bytes, we have to do it in
// smaller chunks instead of all at once
for (uint8_t k = 0; k < length * 2; k += min(length * 2, BUFFER_LENGTH)) {
Wire.beginTransmission(devAddr);
Wire.write(regAddr);
Wire.endTransmission();
Wire.beginTransmission(devAddr);
Wire.requestFrom(devAddr, (uint8_t)(length * 2)); // length=words, this wants bytes
for (uint8_t k = 0; k < length * 2; k += min(length * 2, I2CDEVLIB_WIRE_BUFFER_LENGTH)) {
useWire->beginTransmission(devAddr);
useWire->write(regAddr);
useWire->endTransmission();
useWire->beginTransmission(devAddr);
useWire->requestFrom(devAddr, (uint8_t)(length * 2)); // length=words, this wants bytes
bool msb = true; // starts with MSB, then LSB
for (; Wire.available() && count < length && (timeout == 0 || millis() - t1 < timeout);) {
for (; useWire->available() && count < length && (timeout == 0 || millis() - t1 < timeout);) {
if (msb) {
// first byte is bits 15-8 (MSb=15)
data[count] = Wire.read() << 8;
data[count] = useWire->read() << 8;
} else {
// second byte is bits 7-0 (LSb=0)
data[count] |= Wire.read();
data[count] |= useWire->read();
#ifdef I2CDEV_SERIAL_DEBUG
Serial.print(data[count], HEX);
if (count + 1 < length) Serial.print(" ");
@ -440,7 +442,7 @@ int8_t I2Cdev::readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint1
msb = !msb;
}
Wire.endTransmission();
useWire->endTransmission();
}
#endif
@ -479,11 +481,11 @@ int8_t I2Cdev::readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint1
* @param value New bit value to write
* @return Status of operation (true = success)
*/
bool I2Cdev::writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data) {
bool I2Cdev::writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data, void *wireObj) {
uint8_t b;
readByte(devAddr, regAddr, &b);
readByte(devAddr, regAddr, &b, I2Cdev::readTimeout, wireObj);
b = (data != 0) ? (b | (1 << bitNum)) : (b & ~(1 << bitNum));
return writeByte(devAddr, regAddr, b);
return writeByte(devAddr, regAddr, b, wireObj);
}
/** write a single bit in a 16-bit device register.
@ -493,11 +495,11 @@ bool I2Cdev::writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t
* @param value New bit value to write
* @return Status of operation (true = success)
*/
bool I2Cdev::writeBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t data) {
bool I2Cdev::writeBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t data, void *wireObj) {
uint16_t w;
readWord(devAddr, regAddr, &w);
readWord(devAddr, regAddr, &w, I2Cdev::readTimeout, wireObj);
w = (data != 0) ? (w | (1 << bitNum)) : (w & ~(1 << bitNum));
return writeWord(devAddr, regAddr, w);
return writeWord(devAddr, regAddr, w, wireObj);
}
/** Write multiple bits in an 8-bit device register.
@ -508,7 +510,7 @@ bool I2Cdev::writeBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_
* @param data Right-aligned value to write
* @return Status of operation (true = success)
*/
bool I2Cdev::writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data) {
bool I2Cdev::writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data, void *wireObj) {
// 010 value to write
// 76543210 bit numbers
// xxx args: bitStart=4, length=3
@ -517,13 +519,13 @@ bool I2Cdev::writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8
// 10100011 original & ~mask
// 10101011 masked | value
uint8_t b;
if (readByte(devAddr, regAddr, &b) != 0) {
if (readByte(devAddr, regAddr, &b, I2Cdev::readTimeout, wireObj) != 0) {
uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1);
data <<= (bitStart - length + 1); // shift data into correct position
data &= mask; // zero all non-important bits in data
b &= ~(mask); // zero all important bits in existing byte
b |= data; // combine data with existing byte
return writeByte(devAddr, regAddr, b);
return writeByte(devAddr, regAddr, b, wireObj);
} else {
return false;
}
@ -537,7 +539,7 @@ bool I2Cdev::writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8
* @param data Right-aligned value to write
* @return Status of operation (true = success)
*/
bool I2Cdev::writeBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t data) {
bool I2Cdev::writeBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t data, void *wireObj) {
// 010 value to write
// fedcba9876543210 bit numbers
// xxx args: bitStart=12, length=3
@ -546,13 +548,13 @@ bool I2Cdev::writeBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint
// 1010001110010110 original & ~mask
// 1010101110010110 masked | value
uint16_t w;
if (readWord(devAddr, regAddr, &w) != 0) {
if (readWord(devAddr, regAddr, &w, I2Cdev::readTimeout, wireObj) != 0) {
uint16_t mask = ((1 << length) - 1) << (bitStart - length + 1);
data <<= (bitStart - length + 1); // shift data into correct position
data &= mask; // zero all non-important bits in data
w &= ~(mask); // zero all important bits in existing word
w |= data; // combine data with existing word
return writeWord(devAddr, regAddr, w);
return writeWord(devAddr, regAddr, w, wireObj);
} else {
return false;
}
@ -564,8 +566,8 @@ bool I2Cdev::writeBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint
* @param data New byte value to write
* @return Status of operation (true = success)
*/
bool I2Cdev::writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data) {
return writeBytes(devAddr, regAddr, 1, &data);
bool I2Cdev::writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data, void *wireObj) {
return writeBytes(devAddr, regAddr, 1, &data, wireObj);
}
/** Write single word to a 16-bit device register.
@ -574,8 +576,8 @@ bool I2Cdev::writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data) {
* @param data New word value to write
* @return Status of operation (true = success)
*/
bool I2Cdev::writeWord(uint8_t devAddr, uint8_t regAddr, uint16_t data) {
return writeWords(devAddr, regAddr, 1, &data);
bool I2Cdev::writeWord(uint8_t devAddr, uint8_t regAddr, uint16_t data, void *wireObj) {
return writeWords(devAddr, regAddr, 1, &data, wireObj);
}
/** Write multiple bytes to an 8-bit device register.
@ -585,7 +587,7 @@ bool I2Cdev::writeWord(uint8_t devAddr, uint8_t regAddr, uint16_t data) {
* @param data Buffer to copy new data from
* @return Status of operation (true = success)
*/
bool I2Cdev::writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t* data) {
bool I2Cdev::writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t* data, void *wireObj) {
#ifdef I2CDEV_SERIAL_DEBUG
Serial.print("I2C (0x");
Serial.print(devAddr, HEX);
@ -596,14 +598,20 @@ bool I2Cdev::writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_
Serial.print("...");
#endif
uint8_t status = 0;
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE || I2CDEV_IMPLEMENTATION == I2CDEV_TEENSY_3X_WIRE
TwoWire *useWire = &Wire;
if (wireObj) useWire = (TwoWire *)wireObj;
#endif
#if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE)
Wire.beginTransmission(devAddr);
Wire.send((uint8_t) regAddr); // send address
useWire->beginTransmission(devAddr);
useWire->send((uint8_t) regAddr); // send address
#elif ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100) \
|| (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE && ARDUINO >= 100) \
|| I2CDEV_IMPLEMENTATION == I2CDEV_TEENSY_3X_WIRE)
Wire.beginTransmission(devAddr);
Wire.write((uint8_t) regAddr); // send address
useWire->beginTransmission(devAddr);
useWire->write((uint8_t) regAddr); // send address
#elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE)
Fastwire::beginTransmission(devAddr);
Fastwire::write(regAddr);
@ -614,21 +622,21 @@ bool I2Cdev::writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_
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]);
useWire->send((uint8_t) data[i]);
#elif ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100) \
|| (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE && ARDUINO >= 100) \
|| I2CDEV_IMPLEMENTATION == I2CDEV_TEENSY_3X_WIRE)
Wire.write((uint8_t) data[i]);
useWire->write((uint8_t) data[i]);
#elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE)
Fastwire::write((uint8_t) data[i]);
#endif
}
#if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE)
Wire.endTransmission();
useWire->endTransmission();
#elif ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100) \
|| (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE && ARDUINO >= 100) \
|| I2CDEV_IMPLEMENTATION == I2CDEV_TEENSY_3X_WIRE)
status = Wire.endTransmission();
status = useWire->endTransmission();
#elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE)
Fastwire::stop();
//status = Fastwire::endTransmission();
@ -646,7 +654,7 @@ bool I2Cdev::writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_
* @param data Buffer to copy new data from
* @return Status of operation (true = success)
*/
bool I2Cdev::writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t* data) {
bool I2Cdev::writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t* data, void *wireObj) {
#ifdef I2CDEV_SERIAL_DEBUG
Serial.print("I2C (0x");
Serial.print(devAddr, HEX);
@ -657,14 +665,20 @@ bool I2Cdev::writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16
Serial.print("...");
#endif
uint8_t status = 0;
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE || I2CDEV_IMPLEMENTATION == I2CDEV_TEENSY_3X_WIRE
TwoWire *useWire = &Wire;
if (wireObj) useWire = (TwoWire *)wireObj;
#endif
#if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE)
Wire.beginTransmission(devAddr);
Wire.send(regAddr); // send address
useWire->beginTransmission(devAddr);
useWire->send(regAddr); // send address
#elif ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100) \
|| (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE && ARDUINO >= 100) \
|| I2CDEV_IMPLEMENTATION == I2CDEV_TEENSY_3X_WIRE)
Wire.beginTransmission(devAddr);
Wire.write(regAddr); // send address
useWire->beginTransmission(devAddr);
useWire->write(regAddr); // send address
#elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE)
Fastwire::beginTransmission(devAddr);
Fastwire::write(regAddr);
@ -675,13 +689,13 @@ bool I2Cdev::writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16
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
useWire->send((uint8_t)(data[i] >> 8)); // send MSB
useWire->send((uint8_t)data[i]); // send LSB
#elif ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100) \
|| (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE && ARDUINO >= 100) \
|| I2CDEV_IMPLEMENTATION == I2CDEV_TEENSY_3X_WIRE)
Wire.write((uint8_t)(data[i] >> 8)); // send MSB
Wire.write((uint8_t)data[i]); // send LSB
useWire->write((uint8_t)(data[i] >> 8)); // send MSB
useWire->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
@ -689,11 +703,11 @@ bool I2Cdev::writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16
#endif
}
#if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE)
Wire.endTransmission();
useWire->endTransmission();
#elif ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100) \
|| (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE && ARDUINO >= 100) \
|| I2CDEV_IMPLEMENTATION == I2CDEV_TEENSY_3X_WIRE)
status = Wire.endTransmission();
status = useWire->endTransmission();
#elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE)
Fastwire::stop();
//status = Fastwire::endTransmission();
@ -753,7 +767,7 @@ uint16_t I2Cdev::readTimeout = I2CDEV_DEFAULT_READ_TIMEOUT;
#endif
TWSR = 0; // no prescaler => prescaler = 1
TWBR = ((16000L / khz) - 16) / 2; // change the I2C clock rate
TWBR = F_CPU / 2000 / khz - 8; // change the I2C clock rate
TWCR = 1 << TWEN; // enable twi module, no interrupt
}
@ -993,7 +1007,7 @@ uint16_t I2Cdev::readTimeout = I2CDEV_DEFAULT_READ_TIMEOUT;
TWBR = ((CPU_FREQ / TWI_FREQ) - 16) / 2; // bitrate register
// enable twi module, acks, and twi interrupt
TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA);
TWCR = (1 << TWEN) | (1 << TWIE) | (1 << TWEA);
/* TWEN - TWI Enable Bit
TWIE - TWI Interrupt Enable
@ -1062,7 +1076,7 @@ uint16_t I2Cdev::readTimeout = I2CDEV_DEFAULT_READ_TIMEOUT;
}
void twii_SetStart() {
TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA) | _BV(TWINT) | _BV(TWSTA);
TWCR = (1 << TWEN) | (1 << TWIE) | (1 << TWEA) | (1 << TWINT) | (1 << TWSTA);
}
void twi_write01() {
@ -1145,19 +1159,19 @@ uint16_t I2Cdev::readTimeout = I2CDEV_DEFAULT_READ_TIMEOUT;
void twi_reply(uint8_t ack) {
// transmit master read ready signal, with or without ack
if (ack){
TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWINT) | _BV(TWEA);
TWCR = (1 << TWEN) | (1 << TWIE) | (1 << TWINT) | (1 << TWEA);
} else {
TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWINT);
TWCR = (1 << TWEN) | (1 << TWIE) | (1 << TWINT);
}
}
void twi_stop(void) {
// send stop condition
TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA) | _BV(TWINT) | _BV(TWSTO);
TWCR = (1 << TWEN) | (1 << TWIE) | (1 << TWEA) | (1 << TWINT) | (1 << TWSTO);
// wait for stop condition to be exectued on bus
// TWINT is not set after a stop condition!
while (TWCR & _BV(TWSTO)) {
while (TWCR & (1 << TWSTO)) {
continue;
}
@ -1167,7 +1181,7 @@ uint16_t I2Cdev::readTimeout = I2CDEV_DEFAULT_READ_TIMEOUT;
void twi_releaseBus(void) {
// release bus
TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA) | _BV(TWINT);
TWCR = (1 << TWEN) | (1 << TWIE) | (1 << TWEA) | (1 << TWINT);
// update twi state
twi_state = TWI_READY;

62
lib/mpu6050/I2Cdev.h
View File

@ -3,6 +3,7 @@
// 2013-06-05 by Jeff Rowberg <jeff@rowberg.net>
//
// Changelog:
// 2021-09-28 - allow custom Wire object as transaction function argument
// 2020-01-20 - hardija : complete support for Teensy 3.x
// 2015-10-30 - simondlevy : support i2c_t3 for Teensy3.1
// 2013-05-06 - add Francesco Ferrara's Fastwire v0.24 implementation with small modifications
@ -48,6 +49,11 @@ THE SOFTWARE.
#ifndef _I2CDEV_H_
#define _I2CDEV_H_
// -----------------------------------------------------------------------------
// Enable deprecated pgmspace typedefs in avr-libc
// -----------------------------------------------------------------------------
#define __PROG_TYPES_COMPAT__
// -----------------------------------------------------------------------------
// I2C interface implementation setting
// -----------------------------------------------------------------------------
@ -99,10 +105,26 @@ THE SOFTWARE.
#endif
#ifdef SPARK
#include <spark_wiring_i2c.h>
#include "application.h"
#define ARDUINO 101
#define BUFFER_LENGTH 32
#endif
#ifndef I2CDEVLIB_WIRE_BUFFER_LENGTH
#if defined(I2C_BUFFER_LENGTH)
// Arduino ESP32 core Wire uses this
#define I2CDEVLIB_WIRE_BUFFER_LENGTH I2C_BUFFER_LENGTH
#elif defined(BUFFER_LENGTH)
// Arduino AVR core Wire and many others use this
#define I2CDEVLIB_WIRE_BUFFER_LENGTH BUFFER_LENGTH
#elif defined(SERIAL_BUFFER_SIZE)
// Arduino SAMD core Wire uses this
#define I2CDEVLIB_WIRE_BUFFER_LENGTH SERIAL_BUFFER_SIZE
#else
// should be a safe fallback, though possibly inefficient
#define I2CDEVLIB_WIRE_BUFFER_LENGTH 32
#endif
#endif
// 1000ms default read timeout (modify with "I2Cdev::readTimeout = [ms];")
#define I2CDEV_DEFAULT_READ_TIMEOUT 1000
@ -111,23 +133,23 @@ class I2Cdev {
public:
I2Cdev();
static int8_t readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout);
static int8_t readBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout);
static int8_t readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout);
static int8_t readBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout);
static int8_t readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout);
static int8_t readWord(uint8_t devAddr, uint8_t regAddr, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout);
static int8_t readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout);
static int8_t readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout);
static int8_t readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout, void *wireObj=0);
static int8_t readBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout, void *wireObj=0);
static int8_t readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout, void *wireObj=0);
static int8_t readBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout, void *wireObj=0);
static int8_t readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout, void *wireObj=0);
static int8_t readWord(uint8_t devAddr, uint8_t regAddr, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout, void *wireObj=0);
static int8_t readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout, void *wireObj=0);
static int8_t readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout, void *wireObj=0);
static bool writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data);
static bool writeBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t data);
static bool writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data);
static bool writeBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t data);
static bool writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data);
static bool writeWord(uint8_t devAddr, uint8_t regAddr, uint16_t data);
static bool writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data);
static bool writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data);
static bool writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data, void *wireObj=0);
static bool writeBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t data, void *wireObj=0);
static bool writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data, void *wireObj=0);
static bool writeBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t data, void *wireObj=0);
static bool writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data, void *wireObj=0);
static bool writeWord(uint8_t devAddr, uint8_t regAddr, uint16_t data, void *wireObj=0);
static bool writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, void *wireObj=0);
static bool writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data, void *wireObj=0);
static uint16_t readTimeout;
};
@ -240,7 +262,7 @@ class I2Cdev {
/* TWI Status is in TWSR, in the top 5 bits: TWS7 - TWS3 */
#define TW_STATUS_MASK (_BV(TWS7)|_BV(TWS6)|_BV(TWS5)|_BV(TWS4)|_BV(TWS3))
#define TW_STATUS_MASK ((1 << TWS7)|(1 << TWS6)|(1 << TWS5)|(1 << TWS4)|(1 << TWS3))
#define TW_STATUS (TWSR & TW_STATUS_MASK)
#define TW_START 0x08
#define TW_REP_START 0x10
@ -275,11 +297,11 @@ class I2Cdev {
//#define _SFR_BYTE(sfr) _MMIO_BYTE(_SFR_ADDR(sfr))
#ifndef sbi // set bit
#define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit))
#define sbi(sfr, bit) (_SFR_BYTE(sfr) |= (1 << bit))
#endif // sbi
#ifndef cbi // clear bit
#define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))
#define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~(1 << bit))
#endif // cbi
extern TwoWire Wire;

1148
lib/mpu6050/MPU6050.cpp
View File

File diff suppressed because it is too large Load Diff

227
lib/mpu6050/MPU6050.h
View File

@ -4,6 +4,7 @@
// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
//
// Changelog:
// 2021/09/27 - split implementations out of header files, finally
// ... - ongoing debug release
// NOTE: THIS IS ONLY A PARIAL RELEASE. THIS DEVICE CLASS IS CURRENTLY UNDERGOING ACTIVE
@ -38,12 +39,15 @@ THE SOFTWARE.
#define _MPU6050_H_
#include "I2Cdev.h"
#include "helper_3dmath.h"
// supporting link: http://forum.arduino.cc/index.php?&topic=143444.msg1079517#msg1079517
// also: http://forum.arduino.cc/index.php?&topic=141571.msg1062899#msg1062899s
#ifdef __AVR__
#include <avr/pgmspace.h>
#elif defined(ESP32)
#include <pgmspace.h>
#else
//#define PROGMEM /* empty */
//#define pgm_read_byte(x) (*(x))
@ -431,11 +435,11 @@ THE SOFTWARE.
#define MPU6050_DMP_MEMORY_BANK_SIZE 256
#define MPU6050_DMP_MEMORY_CHUNK_SIZE 16
// note: DMP code memory blocks defined at end of header file
#define MPU6050_FIFO_DEFAULT_TIMEOUT 11000
class MPU6050 {
class MPU6050_Base {
public:
MPU6050(uint8_t address=MPU6050_DEFAULT_ADDRESS);
MPU6050_Base(uint8_t address=MPU6050_DEFAULT_ADDRESS, void *wireObj=0);
void initialize();
bool testConnection();
@ -717,6 +721,8 @@ class MPU6050 {
int8_t GetCurrentFIFOPacket(uint8_t *data, uint8_t length);
void setFIFOByte(uint8_t data);
void getFIFOBytes(uint8_t *data, uint8_t length);
void setFIFOTimeout(uint32_t fifoTimeout);
uint32_t getFIFOTimeout();
// WHO_AM_I register
uint8_t getDeviceID();
@ -827,215 +833,16 @@ class MPU6050 {
void PID(uint8_t ReadAddress, float kP,float kI, uint8_t Loops); // Does the math
void PrintActiveOffsets(); // See the results of the Calibration
// special methods for MotionApps 2.0 implementation
#ifdef MPU6050_INCLUDE_DMP_MOTIONAPPS20
uint8_t dmpInitialize();
bool dmpPacketAvailable();
uint8_t dmpSetFIFORate(uint8_t fifoRate);
uint8_t dmpGetFIFORate();
uint8_t dmpGetSampleStepSizeMS();
uint8_t dmpGetSampleFrequency();
int32_t dmpDecodeTemperature(int8_t tempReg);
// Register callbacks after a packet of FIFO data is processed
//uint8_t dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority);
//uint8_t dmpUnregisterFIFORateProcess(inv_obj_func func);
uint8_t dmpRunFIFORateProcesses();
// Setup FIFO for various output
uint8_t dmpSendQuaternion(uint_fast16_t accuracy);
uint8_t dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy);
uint8_t dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy);
uint8_t dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy);
uint8_t dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy);
uint8_t dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy);
uint8_t dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy);
uint8_t dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy);
uint8_t dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy);
uint8_t dmpSendPacketNumber(uint_fast16_t accuracy);
uint8_t dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy);
uint8_t dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy);
// Get Fixed Point data from FIFO
uint8_t dmpGetAccel(int32_t *data, const uint8_t* packet=0);
uint8_t dmpGetAccel(int16_t *data, const uint8_t* packet=0);
uint8_t dmpGetAccel(VectorInt16 *v, const uint8_t* packet=0);
uint8_t dmpGetQuaternion(int32_t *data, const uint8_t* packet=0);
uint8_t dmpGetQuaternion(int16_t *data, const uint8_t* packet=0);
uint8_t dmpGetQuaternion(Quaternion *q, const uint8_t* packet=0);
uint8_t dmpGet6AxisQuaternion(int32_t *data, const uint8_t* packet=0);
uint8_t dmpGet6AxisQuaternion(int16_t *data, const uint8_t* packet=0);
uint8_t dmpGet6AxisQuaternion(Quaternion *q, const uint8_t* packet=0);
uint8_t dmpGetRelativeQuaternion(int32_t *data, const uint8_t* packet=0);
uint8_t dmpGetRelativeQuaternion(int16_t *data, const uint8_t* packet=0);
uint8_t dmpGetRelativeQuaternion(Quaternion *data, const uint8_t* packet=0);
uint8_t dmpGetGyro(int32_t *data, const uint8_t* packet=0);
uint8_t dmpGetGyro(int16_t *data, const uint8_t* packet=0);
uint8_t dmpGetGyro(VectorInt16 *v, const uint8_t* packet=0);
uint8_t dmpSetLinearAccelFilterCoefficient(float coef);
uint8_t dmpGetLinearAccel(int32_t *data, const uint8_t* packet=0);
uint8_t dmpGetLinearAccel(int16_t *data, const uint8_t* packet=0);
uint8_t dmpGetLinearAccel(VectorInt16 *v, const uint8_t* packet=0);
uint8_t dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity);
uint8_t dmpGetLinearAccelInWorld(int32_t *data, const uint8_t* packet=0);
uint8_t dmpGetLinearAccelInWorld(int16_t *data, const uint8_t* packet=0);
uint8_t dmpGetLinearAccelInWorld(VectorInt16 *v, const uint8_t* packet=0);
uint8_t dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q);
uint8_t dmpGetGyroAndAccelSensor(int32_t *data, const uint8_t* packet=0);
uint8_t dmpGetGyroAndAccelSensor(int16_t *data, const uint8_t* packet=0);
uint8_t dmpGetGyroAndAccelSensor(VectorInt16 *g, VectorInt16 *a, const uint8_t* packet=0);
uint8_t dmpGetGyroSensor(int32_t *data, const uint8_t* packet=0);
uint8_t dmpGetGyroSensor(int16_t *data, const uint8_t* packet=0);
uint8_t dmpGetGyroSensor(VectorInt16 *v, const uint8_t* packet=0);
uint8_t dmpGetControlData(int32_t *data, const uint8_t* packet=0);
uint8_t dmpGetTemperature(int32_t *data, const uint8_t* packet=0);
uint8_t dmpGetGravity(int32_t *data, const uint8_t* packet=0);
uint8_t dmpGetGravity(int16_t *data, const uint8_t* packet=0);
uint8_t dmpGetGravity(VectorInt16 *v, const uint8_t* packet=0);
uint8_t dmpGetGravity(VectorFloat *v, Quaternion *q);
uint8_t dmpGetUnquantizedAccel(int32_t *data, const uint8_t* packet=0);
uint8_t dmpGetUnquantizedAccel(int16_t *data, const uint8_t* packet=0);
uint8_t dmpGetUnquantizedAccel(VectorInt16 *v, const uint8_t* packet=0);
uint8_t dmpGetQuantizedAccel(int32_t *data, const uint8_t* packet=0);
uint8_t dmpGetQuantizedAccel(int16_t *data, const uint8_t* packet=0);
uint8_t dmpGetQuantizedAccel(VectorInt16 *v, const uint8_t* packet=0);
uint8_t dmpGetExternalSensorData(int32_t *data, uint16_t size, const uint8_t* packet=0);
uint8_t dmpGetEIS(int32_t *data, const uint8_t* packet=0);
uint8_t dmpGetEuler(float *data, Quaternion *q);
uint8_t dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity);
// Get Floating Point data from FIFO
uint8_t dmpGetAccelFloat(float *data, const uint8_t* packet=0);
uint8_t dmpGetQuaternionFloat(float *data, const uint8_t* packet=0);
uint8_t dmpProcessFIFOPacket(const unsigned char *dmpData);
uint8_t dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed=NULL);
uint8_t dmpSetFIFOProcessedCallback(void (*func) (void));
uint8_t dmpInitFIFOParam();
uint8_t dmpCloseFIFO();
uint8_t dmpSetGyroDataSource(uint8_t source);
uint8_t dmpDecodeQuantizedAccel();
uint32_t dmpGetGyroSumOfSquare();
uint32_t dmpGetAccelSumOfSquare();
void dmpOverrideQuaternion(long *q);
uint16_t dmpGetFIFOPacketSize();
uint8_t dmpGetCurrentFIFOPacket(uint8_t *data); // overflow proof
#endif
// special methods for MotionApps 4.1 implementation
#ifdef MPU6050_INCLUDE_DMP_MOTIONAPPS41
uint8_t dmpInitialize();
bool dmpPacketAvailable();
uint8_t dmpSetFIFORate(uint8_t fifoRate);
uint8_t dmpGetFIFORate();
uint8_t dmpGetSampleStepSizeMS();
uint8_t dmpGetSampleFrequency();
int32_t dmpDecodeTemperature(int8_t tempReg);
// Register callbacks after a packet of FIFO data is processed
//uint8_t dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority);
//uint8_t dmpUnregisterFIFORateProcess(inv_obj_func func);
uint8_t dmpRunFIFORateProcesses();
// Setup FIFO for various output
uint8_t dmpSendQuaternion(uint_fast16_t accuracy);
uint8_t dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy);
uint8_t dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy);
uint8_t dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy);
uint8_t dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy);
uint8_t dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy);
uint8_t dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy);
uint8_t dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy);
uint8_t dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy);
uint8_t dmpSendPacketNumber(uint_fast16_t accuracy);
uint8_t dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy);
uint8_t dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy);
// Get Fixed Point data from FIFO
uint8_t dmpGetAccel(int32_t *data, const uint8_t* packet=0);
uint8_t dmpGetAccel(int16_t *data, const uint8_t* packet=0);
uint8_t dmpGetAccel(VectorInt16 *v, const uint8_t* packet=0);
uint8_t dmpGetQuaternion(int32_t *data, const uint8_t* packet=0);
uint8_t dmpGetQuaternion(int16_t *data, const uint8_t* packet=0);
uint8_t dmpGetQuaternion(Quaternion *q, const uint8_t* packet=0);
uint8_t dmpGet6AxisQuaternion(int32_t *data, const uint8_t* packet=0);
uint8_t dmpGet6AxisQuaternion(int16_t *data, const uint8_t* packet=0);
uint8_t dmpGet6AxisQuaternion(Quaternion *q, const uint8_t* packet=0);
uint8_t dmpGetRelativeQuaternion(int32_t *data, const uint8_t* packet=0);
uint8_t dmpGetRelativeQuaternion(int16_t *data, const uint8_t* packet=0);
uint8_t dmpGetRelativeQuaternion(Quaternion *data, const uint8_t* packet=0);
uint8_t dmpGetGyro(int32_t *data, const uint8_t* packet=0);
uint8_t dmpGetGyro(int16_t *data, const uint8_t* packet=0);
uint8_t dmpGetGyro(VectorInt16 *v, const uint8_t* packet=0);
uint8_t dmpGetMag(int16_t *data, const uint8_t* packet=0);
uint8_t dmpSetLinearAccelFilterCoefficient(float coef);
uint8_t dmpGetLinearAccel(int32_t *data, const uint8_t* packet=0);
uint8_t dmpGetLinearAccel(int16_t *data, const uint8_t* packet=0);
uint8_t dmpGetLinearAccel(VectorInt16 *v, const uint8_t* packet=0);
uint8_t dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity);
uint8_t dmpGetLinearAccelInWorld(int32_t *data, const uint8_t* packet=0);
uint8_t dmpGetLinearAccelInWorld(int16_t *data, const uint8_t* packet=0);
uint8_t dmpGetLinearAccelInWorld(VectorInt16 *v, const uint8_t* packet=0);
uint8_t dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q);
uint8_t dmpGetGyroAndAccelSensor(int32_t *data, const uint8_t* packet=0);
uint8_t dmpGetGyroAndAccelSensor(int16_t *data, const uint8_t* packet=0);
uint8_t dmpGetGyroAndAccelSensor(VectorInt16 *g, VectorInt16 *a, const uint8_t* packet=0);
uint8_t dmpGetGyroSensor(int32_t *data, const uint8_t* packet=0);
uint8_t dmpGetGyroSensor(int16_t *data, const uint8_t* packet=0);
uint8_t dmpGetGyroSensor(VectorInt16 *v, const uint8_t* packet=0);
uint8_t dmpGetControlData(int32_t *data, const uint8_t* packet=0);
uint8_t dmpGetTemperature(int32_t *data, const uint8_t* packet=0);
uint8_t dmpGetGravity(int32_t *data, const uint8_t* packet=0);
uint8_t dmpGetGravity(int16_t *data, const uint8_t* packet=0);
uint8_t dmpGetGravity(VectorInt16 *v, const uint8_t* packet=0);
uint8_t dmpGetGravity(VectorFloat *v, Quaternion *q);
uint8_t dmpGetUnquantizedAccel(int32_t *data, const uint8_t* packet=0);
uint8_t dmpGetUnquantizedAccel(int16_t *data, const uint8_t* packet=0);
uint8_t dmpGetUnquantizedAccel(VectorInt16 *v, const uint8_t* packet=0);
uint8_t dmpGetQuantizedAccel(int32_t *data, const uint8_t* packet=0);
uint8_t dmpGetQuantizedAccel(int16_t *data, const uint8_t* packet=0);
uint8_t dmpGetQuantizedAccel(VectorInt16 *v, const uint8_t* packet=0);
uint8_t dmpGetExternalSensorData(int32_t *data, uint16_t size, const uint8_t* packet=0);
uint8_t dmpGetEIS(int32_t *data, const uint8_t* packet=0);
uint8_t dmpGetEuler(float *data, Quaternion *q);
uint8_t dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity);
// Get Floating Point data from FIFO
uint8_t dmpGetAccelFloat(float *data, const uint8_t* packet=0);
uint8_t dmpGetQuaternionFloat(float *data, const uint8_t* packet=0);
uint8_t dmpProcessFIFOPacket(const unsigned char *dmpData);
uint8_t dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed=NULL);
uint8_t dmpSetFIFOProcessedCallback(void (*func) (void));
uint8_t dmpInitFIFOParam();
uint8_t dmpCloseFIFO();
uint8_t dmpSetGyroDataSource(uint8_t source);
uint8_t dmpDecodeQuantizedAccel();
uint32_t dmpGetGyroSumOfSquare();
uint32_t dmpGetAccelSumOfSquare();
void dmpOverrideQuaternion(long *q);
uint16_t dmpGetFIFOPacketSize();
#endif
private:
protected:
uint8_t devAddr;
void *wireObj;
uint8_t buffer[14];
#if defined(MPU6050_INCLUDE_DMP_MOTIONAPPS20) or defined(MPU6050_INCLUDE_DMP_MOTIONAPPS41)
uint8_t *dmpPacketBuffer;
uint16_t dmpPacketSize;
#endif
uint32_t fifoTimeout = MPU6050_FIFO_DEFAULT_TIMEOUT;
};
#ifndef I2CDEVLIB_MPU6050_TYPEDEF
#define I2CDEVLIB_MPU6050_TYPEDEF
typedef MPU6050_Base MPU6050;
#endif
#endif /* _MPU6050_H_ */

216
lib/mpu6050/helper_3dmath.h Normal file
View File

@ -0,0 +1,216 @@
// I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class, 3D math helper
// 6/5/2012 by Jeff Rowberg <jeff@rowberg.net>
// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
//
// Changelog:
// 2012-06-05 - add 3D math helper file to DMP6 example sketch
/* ============================================
I2Cdev device library code is placed under the MIT license
Copyright (c) 2012 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
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
===============================================
*/
#ifndef _HELPER_3DMATH_H_
#define _HELPER_3DMATH_H_
class Quaternion {
public:
float w;
float x;
float y;
float z;
Quaternion() {
w = 1.0f;
x = 0.0f;
y = 0.0f;
z = 0.0f;
}
Quaternion(float nw, float nx, float ny, float nz) {
w = nw;
x = nx;
y = ny;
z = nz;
}
Quaternion getProduct(Quaternion q) {
// Quaternion multiplication is defined by:
// (Q1 * Q2).w = (w1w2 - x1x2 - y1y2 - z1z2)
// (Q1 * Q2).x = (w1x2 + x1w2 + y1z2 - z1y2)
// (Q1 * Q2).y = (w1y2 - x1z2 + y1w2 + z1x2)
// (Q1 * Q2).z = (w1z2 + x1y2 - y1x2 + z1w2
return Quaternion(
w*q.w - x*q.x - y*q.y - z*q.z, // new w
w*q.x + x*q.w + y*q.z - z*q.y, // new x
w*q.y - x*q.z + y*q.w + z*q.x, // new y
w*q.z + x*q.y - y*q.x + z*q.w); // new z
}
Quaternion getConjugate() {
return Quaternion(w, -x, -y, -z);
}
float getMagnitude() {
return sqrt(w*w + x*x + y*y + z*z);
}
void normalize() {
float m = getMagnitude();
w /= m;
x /= m;
y /= m;
z /= m;
}
Quaternion getNormalized() {
Quaternion r(w, x, y, z);
r.normalize();
return r;
}
};
class VectorInt16 {
public:
int16_t x;
int16_t y;
int16_t z;
VectorInt16() {
x = 0;
y = 0;
z = 0;
}
VectorInt16(int16_t nx, int16_t ny, int16_t nz) {
x = nx;
y = ny;
z = nz;
}
float getMagnitude() {
return sqrt(x*x + y*y + z*z);
}
void normalize() {
float m = getMagnitude();
x /= m;
y /= m;
z /= m;
}
VectorInt16 getNormalized() {
VectorInt16 r(x, y, z);
r.normalize();
return r;
}
void rotate(Quaternion *q) {
// http://www.cprogramming.com/tutorial/3d/quaternions.html
// http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/transforms/index.htm
// http://content.gpwiki.org/index.php/OpenGL:Tutorials:Using_Quaternions_to_represent_rotation
// ^ or: http://webcache.googleusercontent.com/search?q=cache:xgJAp3bDNhQJ:content.gpwiki.org/index.php/OpenGL:Tutorials:Using_Quaternions_to_represent_rotation&hl=en&gl=us&strip=1
// P_out = q * P_in * conj(q)
// - P_out is the output vector
// - q is the orientation quaternion
// - P_in is the input vector (a*aReal)
// - conj(q) is the conjugate of the orientation quaternion (q=[w,x,y,z], q*=[w,-x,-y,-z])
Quaternion p(0, x, y, z);
// quaternion multiplication: q * p, stored back in p
p = q -> getProduct(p);
// quaternion multiplication: p * conj(q), stored back in p
p = p.getProduct(q -> getConjugate());
// p quaternion is now [0, x', y', z']
x = p.x;
y = p.y;
z = p.z;
}
VectorInt16 getRotated(Quaternion *q) {
VectorInt16 r(x, y, z);
r.rotate(q);
return r;
}
};
class VectorFloat {
public:
float x;
float y;
float z;
VectorFloat() {
x = 0;
y = 0;
z = 0;
}
VectorFloat(float nx, float ny, float nz) {
x = nx;
y = ny;
z = nz;
}
float getMagnitude() {
return sqrt(x*x + y*y + z*z);
}
void normalize() {
float m = getMagnitude();
x /= m;
y /= m;
z /= m;
}
VectorFloat getNormalized() {
VectorFloat r(x, y, z);
r.normalize();
return r;
}
void rotate(Quaternion *q) {
Quaternion p(0, x, y, z);
// quaternion multiplication: q * p, stored back in p
p = q -> getProduct(p);
// quaternion multiplication: p * conj(q), stored back in p
p = p.getProduct(q -> getConjugate());
// p quaternion is now [0, x', y', z']
x = p.x;
y = p.y;
z = p.z;
}
VectorFloat getRotated(Quaternion *q) {
VectorFloat r(x, y, z);
r.rotate(q);
return r;
}
};
#endif /* _HELPER_3DMATH_H_ */

22
platformio.ini
View File

@ -19,6 +19,7 @@ platform = espressif8266
framework = arduino
board = d1_mini
build_unflags =
0src_build_flags = -Wunused-variable -Wregister -Wchar-subscripts
build_flags = #-O0 -Wl,-Map,output.map
-D BAUD=${common_env_data.monitor_speed}
-D ACTIVATE_OTA
@ -33,16 +34,17 @@ build_flags = #-O0 -Wl,-Map,output.map
-D USER_SSID=\""\"" # =\""myssid\""
-D USER_SSID_PWD=\""\"" # =\""mypwd\""
-D CFG_APPVER="\"0.4.0\""
lib_deps =
# https://github.com/jrowberg/i2cdevlib.git # Using local copy of this library
https://github.com/codeplea/tinyexpr
https://github.com/graphitemaster/incbin
https://github.com/khoih-prog/ESP_DoubleResetDetector
https://github.com/tzapu/WiFiManager
https://github.com/thijse/Arduino-Log
https://github.com/bblanchon/ArduinoJson
https://github.com/PaulStoffregen/OneWire
https://github.com/milesburton/Arduino-Temperature-Control-Library
lib_deps = # Switched to forks for better version control.
# Using local copy of this library
#https://github.com/mp-se/i2cdevlib # https://github.com/jrowberg/i2cdevlib.git
https://github.com/mp-se/tinyexpr # https://github.com/codeplea/tinyexpr
https://github.com/mp-se/incbin # https://github.com/graphitemaster/incbin
https://github.com/mp-se/ESP_DoubleResetDetector # https://github.com/khoih-prog/ESP_DoubleResetDetector
https://github.com/mp-se/WiFiManager # https://github.com/tzapu/WiFiManager
https://github.com/mp-se/Arduino-Log # https://github.com/thijse/Arduino-Log
https://github.com/mp-se/ArduinoJson # https://github.com/bblanchon/ArduinoJson
https://github.com/mp-se/OneWire # https://github.com/PaulStoffregen/OneWire
https://github.com/mp-se/Arduino-Temperature-Control-Library # https://github.com/milesburton/Arduino-Temperature-Control-Library
[env:gravity-debug]
upload_speed = ${common_env_data.upload_speed}