using MPU6050 no-interrupt gives erratic accel readings
tonyvr4 opened this issue · 7 comments
I am using your awesome lib to interface with an MPU6050 on an ESP32 without interrupt pin.
The basic code reads data from the MPU6050 and then displays it on the OLED
However the readings are very erratic. Even wth the MPU taped to a table to give stability, the readings are very erratic and occasionally read close to 2g without any movement at all
// ACCEL_to_OLED
//
#include "heltec.h"
#include "SSD1306Ascii.h"
#include "SSD1306AsciiWire.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include "Wire.h"
SSD1306AsciiWire oled;
MPU6050 mpu;
bool dmpReady = false;
uint8_t devStatus;
uint16_t packetSize;
uint16_t fifoCount;
uint8_t fifoBuffer[64];
float MaxGx = 0, MaxGy = 0, MaxGz = 0;
Quaternion q; // [w, x, y, z] quaternion container
void setup() {
Heltec.begin(true /*DisplayEnable Enable*/, false /*LoRa Enable*/, true /*Serial Enable*/);
oled.begin(&Adafruit128x64, 0x3C, 16);
oled.setFont(System5x7);
oled.clear();
Wire.begin(); //pin 21 and 22 for ESP32
Wire.setClock(400000);
Serial.begin(115200);
Serial.println(F("Initializing I2C devices..."));
mpu.initialize();
Serial.println(F("Testing device connections..."));
Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
Serial.println(F("Initializing DMP..."));
oled.setCursor(0,0);
oled.println("Calibrate Acc/Gyro");
devStatus = mpu.dmpInitialize();
mpu.setXGyroOffset(220);
mpu.setYGyroOffset(76);
mpu.setZGyroOffset(-85);
mpu.setZAccelOffset(1788); // 1688 factory default for my test chip
if (devStatus == 0) {
mpu.CalibrateAccel(6);
mpu.CalibrateGyro(6);
mpu.PrintActiveOffsets();
Serial.println(F("Enabling DMP..."));
oled.print("Accel/Gyro Ready");
delay(1000);
oled.clear();
mpu.setDMPEnabled(true);
Serial.println(F("DMP ready!"));
dmpReady = true;
packetSize = mpu.dmpGetFIFOPacketSize();
} else {
// ERROR!
// 1 = initial memory load failed
// 2 = DMP configuration updates failed
// (if it's going to break, usually the code will be 1)
Serial.print(F("DMP Initialization failed (code "));
Serial.print(devStatus);
Serial.println(F(")"));
}
}
void loop() {
// if MPU6050nprogramming failed, don't do anything
if (!dmpReady) return;
while (fifoCount < packetSize) {
if (fifoCount < packetSize) {
// try to get out of the infinite loop
fifoCount = mpu.getFIFOCount();
}
}
fifoCount = mpu.getFIFOCount();
if(fifoCount < packetSize){
}
else if (fifoCount >= 1024) {
// reset so we can continue cleanly
mpu.resetFIFO();
// fifoCount = mpu.getFIFOCount(); // will be zero after reset
Serial.println(F("FIFO overflow!"));
}
while(fifoCount >= packetSize){
mpu.getFIFOBytes(fifoBuffer, packetSize);
fifoCount -= packetSize;
}
mpu.dmpGetQuaternion(&q, fifoBuffer);
if (abs(q.x) > abs(MaxGx))
{
MaxGx = q.x;
} else{
MaxGx = MaxGx;
}
if (abs(q.y) > abs(MaxGy))
{
MaxGy = q.y;
} else{
MaxGy = MaxGy;
}
if (abs(q.z) > abs(MaxGz))
{
MaxGz = q.z;
} else{
MaxGz = MaxGz;
}
oled.setCursor(0,0);
oled.clearToEOL();
oled.println("ACCEL X Y Z");
oled.setCursor(0,1);
oled.clearToEOL();
oled.setCursor(20,1);
oled.print(q.x,2);
oled.setCursor(60,1);
oled.print(q.y,2);
oled.setCursor(95,1);
oled.println(q.z,2);
oled.setCursor(0,4);
oled.println("MaxG X Y Z");
oled.setCursor(0,5);
oled.clearToEOL();
oled.setCursor(20,5);
oled.print(MaxGx);
oled.setCursor(60,5);
oled.print(MaxGy);
oled.setCursor(95,5);
oled.println(MaxGz);
}
the polling it can read "junk data" because it is always trying to read, please keep the issue in #13 , please don't open an issue every time
Sorry. I thought it would be a new issue. Can you merge this with # 13 to avoid confusion?
How can I eliminate the garbage data ?
I going to close #13 and we are still here
for the garbage data, you can follow the example https://github.com/ElectronicCats/mpu6050/blob/master/examples/IMU_Zero/IMU_Zero.ino that does not have interrupt
Would it be preferable to use an interrupt?
I am also reading data from a GPS module and I thought the interrupt would cause dropped GPS data
I will look at that example. Thanks
I have done a little more testing and the MPU6050 is stable for a few seconds then it goes nuts for a while. Readings are near maximum. Then it settles down again
I traced the problem to the block of code below. It seems to be an issue with improperly resetting the FIFO
`// 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
// .
// .
// .
// if you are really paranoid you can frequently test in between other
// stuff to see if mpuInterrupt is true, and if so, "break;" from the
// while() loop to immediately process the MPU data
// .
// .
// .
}
// reset interrupt flag and get INT_STATUS byte
mpuInterrupt = false;
mpuIntStatus = mpu.getIntStatus();
// get current FIFO count
fifoCount = mpu.getFIFOCount();
if(fifoCount < packetSize){
//Lets go back and wait for another interrupt. We shouldn't be here, we got an interrupt from another event
// This is blocking so don't do it while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
}
// check for overflow (this should never happen unless our code is too inefficient)
else if ((mpuIntStatus & _BV(MPU6050_INTERRUPT_FIFO_OFLOW_BIT)) || fifoCount >= 1024) {
// reset so we can continue cleanly
mpu.resetFIFO();
// fifoCount = mpu.getFIFOCount(); // will be zero after reset no need to ask
Serial.println(F("FIFO overflow!"));
// otherwise, check for DMP data ready interrupt (this should happen frequently)
} else if (mpuIntStatus & _BV(MPU6050_INTERRUPT_DMP_INT_BIT)) {
// read a packet from FIFO
while(fifoCount >= packetSize){ // Lets catch up to NOW, someone is using the dreaded delay()!
mpu.getFIFOBytes(fifoBuffer, packetSize);
// track FIFO count here in case there is > 1 packet available
// (this lets us immediately read more without waiting for an interrupt)
fifoCount -= packetSize;`
Since I am not using an interrupt I changed the code to this:
`// wait for MPU extra packet(s) available
while (fifoCount < packetSize) {
if (fifoCount < packetSize) {
// try to get out of the infinite loop
fifoCount = mpu.getFIFOCount();
}
}
// get current FIFO count
fifoCount = mpu.getFIFOCount();
if(fifoCount < packetSize){
}
// check for FIFO overflow
else if (fifoCount >= 1024) {
// reset so we can continue cleanly
mpu.resetFIFO();
// fifoCount = mpu.getFIFOCount(); // will be zero after reset
Serial.println(F("FIFO overflow!"));
}
// read a packet from FIFO
while(fifoCount >= packetSize){
mpu.getFIFOBytes(fifoBuffer, packetSize);
// track FIFO count in case there is > 1 packet available
fifoCount -= packetSize;
}`
If I add an extra FIFO reset the readings are stable. How can I modify this code from the DMP6 example to work properly in this case?