Commit e4559e0d authored by larsm's avatar larsm

Updated libs: MPU6050, I2Cdev

parent a9ad0ad7
......@@ -45,7 +45,7 @@ THE SOFTWARE.
#include "I2Cdev.h"
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE
#ifdef I2CDEV_IMPLEMENTATION_WARNINGS
#if ARDUINO < 100
......@@ -87,6 +87,11 @@ 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() {
......@@ -217,7 +222,7 @@ int8_t I2Cdev::readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8
int8_t count = 0;
uint32_t t1 = millis();
#if (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE)
#if (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE)
#if (ARDUINO < 100)
// Arduino v00xx (before v1.0), Wire library
......@@ -225,7 +230,7 @@ int8_t I2Cdev::readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8
// 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
// smaller chunks instead of all at once
for (uint8_t k = 0; k < length; k += min(length, BUFFER_LENGTH)) {
for (uint8_t k = 0; k < length; k += min((int)length, BUFFER_LENGTH)) {
Wire.beginTransmission(devAddr);
Wire.send(regAddr);
Wire.endTransmission();
......@@ -249,7 +254,7 @@ int8_t I2Cdev::readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8
// 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
// smaller chunks instead of all at once
for (uint8_t k = 0; k < length; k += min(length, BUFFER_LENGTH)) {
for (uint8_t k = 0; k < length; k += min((int)length, BUFFER_LENGTH)) {
Wire.beginTransmission(devAddr);
Wire.write(regAddr);
Wire.endTransmission();
......@@ -273,7 +278,7 @@ int8_t I2Cdev::readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8
// 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
// smaller chunks instead of all at once
for (uint8_t k = 0; k < length; k += min(length, BUFFER_LENGTH)) {
for (uint8_t k = 0; k < length; k += min((int)length, BUFFER_LENGTH)) {
Wire.beginTransmission(devAddr);
Wire.write(regAddr);
Wire.endTransmission();
......@@ -337,7 +342,7 @@ int8_t I2Cdev::readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint1
int8_t count = 0;
uint32_t t1 = millis();
#if (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE)
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE
#if (ARDUINO < 100)
// Arduino v00xx (before v1.0), Wire library
......@@ -594,7 +599,8 @@ bool I2Cdev::writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_
#if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE)
Wire.beginTransmission(devAddr);
Wire.send((uint8_t) regAddr); // send address
#elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100)
#elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100 \
|| I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE && ARDUINO >= 100)
Wire.beginTransmission(devAddr);
Wire.write((uint8_t) regAddr); // send address
#elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE)
......@@ -608,16 +614,18 @@ bool I2Cdev::writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_
#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_ARDUINO_WIRE && ARDUINO >= 100 \
|| I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE && ARDUINO >= 100)
Wire.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();
#elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100)
status = Wire.endTransmission();
#elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100 \
|| I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE && ARDUINO >= 100)
status = Wire.endTransmission();
#elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE)
Fastwire::stop();
//status = Fastwire::endTransmission();
......@@ -649,8 +657,9 @@ bool I2Cdev::writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16
#if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE)
Wire.beginTransmission(devAddr);
Wire.send(regAddr); // send address
#elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100)
Wire.beginTransmission(devAddr);
#elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100 \
|| I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE && ARDUINO >= 100)
Wire.beginTransmission(devAddr);
Wire.write(regAddr); // send address
#elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE)
Fastwire::beginTransmission(devAddr);
......@@ -664,8 +673,9 @@ bool I2Cdev::writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16
#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
#elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100 \
|| I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE && 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
......@@ -675,8 +685,9 @@ bool I2Cdev::writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16
}
#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_ARDUINO_WIRE && ARDUINO >= 100 \
|| I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE && ARDUINO >= 100)
status = Wire.endTransmission();
#elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE)
Fastwire::stop();
//status = Fastwire::endTransmission();
......
......@@ -47,13 +47,12 @@ THE SOFTWARE.
#ifndef _I2CDEV_H_
#define _I2CDEV_H_
#define BUFFER_LENGTH 32
// -----------------------------------------------------------------------------
// I2C interface implementation setting
// -----------------------------------------------------------------------------
#ifndef I2CDEV_IMPLEMENTATION
#define I2CDEV_IMPLEMENTATION I2CDEV_ARDUINO_WIRE
//#define I2CDEV_IMPLEMENTATION I2CDEV_BUILTIN_SBWIRE
//#define I2CDEV_IMPLEMENTATION I2CDEV_BUILTIN_FASTWIRE
#endif // I2CDEV_IMPLEMENTATION
......@@ -69,6 +68,7 @@ THE SOFTWARE.
// ^^^ NBWire implementation is still buggy w/some interrupts!
#define I2CDEV_BUILTIN_FASTWIRE 3 // FastWire object from Francesco Ferrara's project
#define I2CDEV_I2CMASTER_LIBRARY 4 // I2C object from DSSCircuits I2C-Master Library at https://github.com/DSSCircuits/I2C-Master-Library
#define I2CDEV_BUILTIN_SBWIRE 5 // I2C object from Shuning (Steve) Bian's SBWire Library at https://github.com/freespace/SBWire
// -----------------------------------------------------------------------------
// Arduino-style "Serial.print" debug constant (uncomment to enable)
......@@ -87,6 +87,9 @@ THE SOFTWARE.
#if I2CDEV_IMPLEMENTATION == I2CDEV_I2CMASTER_LIBRARY
#include <I2C.h>
#endif
#if I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE
#include "SBWire.h"
#endif
#endif
#ifdef SPARK
......
......@@ -36,21 +36,13 @@ THE SOFTWARE.
#include "MPU6050.h"
/** Default constructor, uses default I2C address.
* @see MPU6050_DEFAULT_ADDRESS
*/
MPU6050::MPU6050() {
devAddr = MPU6050_DEFAULT_ADDRESS;
}
/** Specific address constructor.
* @param address I2C address
* @param address I2C address, uses default I2C address if none is specified
* @see MPU6050_DEFAULT_ADDRESS
* @see MPU6050_ADDRESS_AD0_LOW
* @see MPU6050_ADDRESS_AD0_HIGH
*/
MPU6050::MPU6050(uint8_t address) {
devAddr = address;
MPU6050::MPU6050(uint8_t address):devAddr(address) {
}
/** Power on and prepare for general usage.
......
......@@ -435,8 +435,7 @@ THE SOFTWARE.
class MPU6050 {
public:
MPU6050();
MPU6050(uint8_t address);
MPU6050(uint8_t address=MPU6050_DEFAULT_ADDRESS);
void initialize();
bool testConnection();
......@@ -459,15 +458,15 @@ class MPU6050 {
uint8_t getFullScaleGyroRange();
void setFullScaleGyroRange(uint8_t range);
// SELF_TEST registers
uint8_t getAccelXSelfTestFactoryTrim();
uint8_t getAccelYSelfTestFactoryTrim();
uint8_t getAccelZSelfTestFactoryTrim();
// SELF_TEST registers
uint8_t getAccelXSelfTestFactoryTrim();
uint8_t getAccelYSelfTestFactoryTrim();
uint8_t getAccelZSelfTestFactoryTrim();
uint8_t getGyroXSelfTestFactoryTrim();
uint8_t getGyroYSelfTestFactoryTrim();
uint8_t getGyroZSelfTestFactoryTrim();
uint8_t getGyroXSelfTestFactoryTrim();
uint8_t getGyroYSelfTestFactoryTrim();
uint8_t getGyroZSelfTestFactoryTrim();
// ACCEL_CONFIG register
bool getAccelXSelfTest();
void setAccelXSelfTest(bool enabled);
......@@ -823,8 +822,6 @@ class MPU6050 {
// special methods for MotionApps 2.0 implementation
#ifdef MPU6050_INCLUDE_DMP_MOTIONAPPS20
uint8_t *dmpPacketBuffer;
uint16_t dmpPacketSize;
uint8_t dmpInitialize();
bool dmpPacketAvailable();
......@@ -924,8 +921,6 @@ class MPU6050 {
// special methods for MotionApps 4.1 implementation
#ifdef MPU6050_INCLUDE_DMP_MOTIONAPPS41
uint8_t *dmpPacketBuffer;
uint16_t dmpPacketSize;
uint8_t dmpInitialize();
bool dmpPacketAvailable();
......@@ -1027,6 +1022,10 @@ class MPU6050 {
private:
uint8_t devAddr;
uint8_t buffer[14];
#if defined(MPU6050_INCLUDE_DMP_MOTIONAPPS20) or defined(MPU6050_INCLUDE_DMP_MOTIONAPPS41)
uint8_t *dmpPacketBuffer;
uint16_t dmpPacketSize;
#endif
};
#endif /* _MPU6050_H_ */
......@@ -269,6 +269,10 @@ const unsigned char dmpMemory[MPU6050_DMP_CODE_SIZE] PROGMEM = {
0xB9, 0xA7, 0xF1, 0x26, 0x26, 0x26, 0xD8, 0xD8, 0xFF
};
#ifndef MPU6050_DMP_FIFO_RATE_DIVISOR
#define MPU6050_DMP_FIFO_RATE_DIVISOR 0x01
#endif
// thanks to Noah Zerkin for piecing this stuff together!
const unsigned char dmpConfig[MPU6050_DMP_CONFIG_SIZE] PROGMEM = {
// BANK OFFSET LENGTH [DATA]
......@@ -302,7 +306,7 @@ const unsigned char dmpConfig[MPU6050_DMP_CONFIG_SIZE] PROGMEM = {
0x07, 0x46, 0x01, 0x9A, // CFG_GYRO_SOURCE inv_send_gyro
0x07, 0x47, 0x04, 0xF1, 0x28, 0x30, 0x38, // CFG_9 inv_send_gyro -> inv_construct3_fifo
0x07, 0x6C, 0x04, 0xF1, 0x28, 0x30, 0x38, // CFG_12 inv_send_accel -> inv_construct3_fifo
0x02, 0x16, 0x02, 0x00, 0x01 // D_0_22 inv_set_fifo_rate
0x02, 0x16, 0x02, 0x00, MPU6050_DMP_FIFO_RATE_DIVISOR // D_0_22 inv_set_fifo_rate
// This very last 0x01 WAS a 0x09, which drops the FIFO rate down to 20 Hz. 0x07 is 25 Hz,
// 0x01 is 100Hz. Going faster than 100Hz (0x00=200Hz) tends to result in very noisy data.
......@@ -395,7 +399,7 @@ uint8_t MPU6050::dmpInitialize() {
setClockSource(MPU6050_CLOCK_PLL_ZGYRO);
DEBUG_PRINTLN(F("Setting DMP and FIFO_OFLOW interrupts enabled..."));
setIntEnabled(0x12);
setIntEnabled(1<<MPU6050_INTERRUPT_FIFO_OFLOW_BIT|1<<MPU6050_INTERRUPT_DMP_INT_BIT);
DEBUG_PRINTLN(F("Setting sample rate to 200Hz..."));
setRate(4); // 1khz / (1 + 4) = 200 Hz
......@@ -703,6 +707,8 @@ uint8_t MPU6050::dmpGetEuler(float *data, Quaternion *q) {
data[2] = atan2(2*q -> y*q -> z - 2*q -> w*q -> x, 2*q -> w*q -> w + 2*q -> z*q -> z - 1); // phi
return 0;
}
#ifdef USE_OLD_DMPGETYAWPITCHROLL
uint8_t MPU6050::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) {
// yaw: (about Z axis)
data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1);
......@@ -712,6 +718,24 @@ uint8_t MPU6050::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gra
data[2] = atan(gravity -> y / sqrt(gravity -> x*gravity -> x + gravity -> z*gravity -> z));
return 0;
}
#else
uint8_t MPU6050::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) {
// yaw: (about Z axis)
data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1);
// pitch: (nose up/down, about Y axis)
data[1] = atan2(gravity -> x , sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z));
// roll: (tilt left/right, about X axis)
data[2] = atan2(gravity -> y , gravity -> z);
if (gravity -> z < 0) {
if(data[1] > 0) {
data[1] = PI - data[1];
} else {
data[1] = -PI - data[1];
}
}
return 0;
}
#endif
// uint8_t MPU6050::dmpGetAccelFloat(float *data, const uint8_t* packet);
// uint8_t MPU6050::dmpGetQuaternionFloat(float *data, const uint8_t* packet);
......
......@@ -262,6 +262,10 @@ const unsigned char dmpMemory[MPU6050_DMP_CODE_SIZE] PROGMEM = {
0xDC, 0xB9, 0xA7, 0xF1, 0x26, 0x26, 0x26, 0xD8, 0xD8, 0xFF
};
#ifndef MPU6050_DMP_FIFO_RATE_DIVISOR
#define MPU6050_DMP_FIFO_RATE_DIVISOR 0x03
#endif
const unsigned char dmpConfig[MPU6050_DMP_CONFIG_SIZE] PROGMEM = {
// BANK OFFSET LENGTH [DATA]
0x02, 0xEC, 0x04, 0x00, 0x47, 0x7D, 0x1A, // ?
......@@ -302,9 +306,9 @@ const unsigned char dmpConfig[MPU6050_DMP_CONFIG_SIZE] PROGMEM = {
0x07, 0x67, 0x01, 0x9A, // ?
0x07, 0x68, 0x04, 0xF1, 0x28, 0x30, 0x38, // CFG_12 inv_send_accel -> inv_construct3_fifo
0x07, 0x8D, 0x04, 0xF1, 0x28, 0x30, 0x38, // ??? CFG_12 inv_send_mag -> inv_construct3_fifo
0x02, 0x16, 0x02, 0x00, 0x03 // D_0_22 inv_set_fifo_rate
0x02, 0x16, 0x02, 0x00, MPU6050_DMP_FIFO_RATE_DIVISOR // D_0_22 inv_set_fifo_rate
// This very last 0x01 WAS a 0x09, which drops the FIFO rate down to 20 Hz. 0x07 is 25 Hz,
// This very last 0x03 WAS a 0x09, which drops the FIFO rate down to 20 Hz. 0x07 is 25 Hz,
// 0x01 is 100Hz. Going faster than 100Hz (0x00=200Hz) tends to result in very noisy data.
// DMP output frequency is calculated easily using this equation: (200Hz / (1 + value))
......@@ -432,7 +436,7 @@ uint8_t MPU6050::dmpInitialize() {
DEBUG_PRINTLN(F("Success! DMP configuration written and verified."));
DEBUG_PRINTLN(F("Setting DMP and FIFO_OFLOW interrupts enabled..."));
setIntEnabled(0x12);
setIntEnabled(1<<MPU6050_INTERRUPT_FIFO_OFLOW_BIT|1<<MPU6050_INTERRUPT_DMP_INT_BIT);
DEBUG_PRINTLN(F("Setting sample rate to 200Hz..."));
setRate(4); // 1khz / (1 + 4) = 200 Hz
......@@ -808,6 +812,8 @@ uint8_t MPU6050::dmpGetEuler(float *data, Quaternion *q) {
data[2] = atan2(2*q -> y*q -> z - 2*q -> w*q -> x, 2*q -> w*q -> w + 2*q -> z*q -> z - 1); // phi
return 0;
}
#ifdef USE_OLD_DMPGETYAWPITCHROLL
uint8_t MPU6050::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) {
// yaw: (about Z axis)
data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1);
......@@ -817,6 +823,24 @@ uint8_t MPU6050::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gra
data[2] = atan(gravity -> y / sqrt(gravity -> x*gravity -> x + gravity -> z*gravity -> z));
return 0;
}
#else
uint8_t MPU6050::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) {
// yaw: (about Z axis)
data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1);
// pitch: (nose up/down, about Y axis)
data[1] = atan2(gravity -> x , sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z));
// roll: (tilt left/right, about X axis)
data[2] = atan2(gravity -> y , gravity -> z);
if(gravity->z<0) {
if(data[1]>0) {
data[1] = PI - data[1];
} else {
data[1] = -PI - data[1];
}
}
return 0;
}
#endif
// uint8_t MPU6050::dmpGetAccelFloat(float *data, const uint8_t* packet);
// uint8_t MPU6050::dmpGetQuaternionFloat(float *data, const uint8_t* packet);
......
......@@ -3,6 +3,7 @@
// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
//
// Changelog:
// 2016-04-18 - Eliminated a potential infinite loop
// 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
......@@ -212,7 +213,9 @@ void setup() {
mpu.setDMPEnabled(true);
// enable Arduino interrupt detection
Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
Serial.print(F("Enabling interrupt detection (Arduino external interrupt "));
Serial.print(digitalPinToInterrupt(INTERRUPT_PIN));
Serial.println(F(")..."));
attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus();
......@@ -248,6 +251,10 @@ void loop() {
// wait for MPU interrupt or extra packet(s) available
while (!mpuInterrupt && fifoCount < packetSize) {
if (mpuInterrupt && fifoCount < packetSize) {
// try to get out of the infinite loop
fifoCount = mpu.getFIFOCount();
}
// other program behavior stuff here
// .
// .
......@@ -268,13 +275,14 @@ void loop() {
fifoCount = mpu.getFIFOCount();
// check for overflow (this should never happen unless our code is too inefficient)
if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
if ((mpuIntStatus & _BV(MPU6050_INTERRUPT_FIFO_OFLOW_BIT)) || fifoCount >= 1024) {
// reset so we can continue cleanly
mpu.resetFIFO();
fifoCount = mpu.getFIFOCount();
Serial.println(F("FIFO overflow!"));
// otherwise, check for DMP data ready interrupt (this should happen frequently)
} else if (mpuIntStatus & 0x02) {
} else if (mpuIntStatus & _BV(MPU6050_INTERRUPT_DMP_INT_BIT)) {
// wait for correct available data length, should be a VERY short wait
while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
......
......@@ -4,8 +4,9 @@
// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
//
// Changelog:
// 2016-04-18 - Eliminated a potential infinite loop
// 2016-02-28 - Cleaned up code to be in line with other example codes
// - Added Ethernet outputs for Quaternion, Euler, RealAccel, WorldAccel
// - Added Ethernet outputs for Quaternion, Euler, RealAccel, WorldAccel
// 2016-02-27 - Initial working code compiled
// Bugs:
// - There is still a hangup after some time, though it only occurs when you are reading data from the website.
......@@ -218,7 +219,9 @@ void setup() {
mpu.setDMPEnabled(true);
// enable Arduino interrupt detection
Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
Serial.print(F("Enabling interrupt detection (Arduino external interrupt "));
Serial.print(digitalPinToInterrupt(INTERRUPT_PIN));
Serial.println(F(")..."));
attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus();
......@@ -236,7 +239,6 @@ void setup() {
Serial.print(F("DMP Initialization failed (code "));
Serial.print(devStatus);
Serial.println(F(")"));
return;
}
// configure LED for output
......@@ -252,9 +254,14 @@ void setup() {
void loop() {
// if programming failed, don't try to do anything
if (!dmpReady) return;
wdt_reset();//Resets the watchdog timer. If the timer is not reset, and the timer expires, a watchdog-initiated device reset will occur.
// wait for MPU interrupt or extra packet(s) available
while (!mpuInterrupt && fifoCount < packetSize) {
if (mpuInterrupt && fifoCount < packetSize) {
// try to get out of the infinite loop
fifoCount = mpu.getFIFOCount();
}
// other program behavior stuff here
// .
// .
......@@ -274,13 +281,14 @@ void loop() {
fifoCount = mpu.getFIFOCount();
// check for overflow (this should never happen unless our code is too inefficient)
if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
if ((mpuIntStatus & _BV(MPU6050_INTERRUPT_FIFO_OFLOW_BIT)) || fifoCount >= 1024) {
// reset so we can continue cleanly
mpu.resetFIFO();
fifoCount = mpu.getFIFOCount();
Serial.println(F("FIFO overflow!"));
// otherwise, check for DMP data ready interrupt (this should happen frequently)
} else if (mpuIntStatus & 0x02) {
} else if (mpuIntStatus & _BV(MPU6050_INTERRUPT_DMP_INT_BIT)) {
// wait for correct available data length, should be a VERY short wait
while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
......
Markdown is supported
0%
or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment