Jump to content
I2Cdevlib Forums

Recommended Posts

Hi,

 
I am using MPU6050  with your written code in DMP mode on Arduino due and it's working fine stand alone.. But I have to integrate some other things into my code like (Due timer library to generate software interrupt every second, external interrupt on pin 52 to count pulses ). when i tried to integrate them in one code i have FIFO Overflow problem. even though i tried to run it at as low as 20hz by changing the registers value in the "3dmath.h" file.
 
can you please help me out with this problem i am facing. my project is kind of stuck at this point.
 
Thank you.
 
#include <DueTimer.h>
#include "I2Cdev.h"

#include "MPU6050_6Axis_MotionApps20.h"

#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
    #include "Wire.h"
#endif

MPU6050 mpu;

//#define OUTPUT_READABLE_WORLDACCEL
#define OUTPUT_READABLE_REALACCEL

#define LED_PIN 13 // (Arduino is 13, Teensy is 11, Teensy++ is 6)
#define LED_PIN1 12
bool blinkState = false;
bool blinkState1 = false;
// MPU control/status vars
bool dmpReady = false;  // set true if DMP init was successful
uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount;     // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer

// orientation/motion vars
Quaternion q;           // [w, x, y, z]         quaternion container
VectorInt16 aa;         // [x, y, z]            accel sensor measurements
VectorInt16 aaReal;     // [x, y, z]            gravity-free accel sensor measurements
VectorInt16 aaWorld;    // [x, y, z]            world-frame accel sensor measurements
VectorFloat gravity;    // [x, y, z]            gravity vector
float euler[3];         // [psi, theta, phi]    Euler angle container
float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector
double vel_data;
double vel;

double Dummy = 0 ;
double t_s = 0;
double S[2] = {0,0};
double Q[2] = {0.05,0.05};
double R[2] = {0.5, 0.01};
double Y[2] = {0,0};
double X[2] ={0,0};
double P[2] = {0.5,0.5};
double K[2] = {0, 0};

uint32_t timer = 0;
uint32_t dt = 0;
int16_t sample,sped=0;
int16_t ax, ay, az;
int16_t gx, gy, gz;
int16_t sensorValue;

char val;
int count;
bool flag;


// ================================================================
// ===               INTERRUPT DETECTION ROUTINE                ===
// ================================================================

volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
    mpuInterrupt = true;
}



// ================================================================
// ===                      INITIAL SETUP                       ===
// ================================================================

void setup() {
    // join I2C bus (I2Cdev library doesn't do this automatically)
    #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
        Wire.begin();
        Wire1.begin();
        //TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz)
    #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
        Fastwire::setup(400, true);
    #endif

    Serial1.begin(115200);  //115200 or 19200
    while (!Serial1); // wait for Leonardo enumeration, others continue immediately

    Serial1.println(F("Initializing I2C devices..."));
    mpu.initialize();

    // verify connection
    Serial1.println(F("Testing device connections..."));
    Serial1.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));

    // wait for ready
   // Serial1.println(F("\nSend any character to begin DMP programming and demo: "));
   // while (Serial1.available() && Serial1.read()); // empty buffer
   // while (!Serial1.available());                 // wait for data
   // while (Serial1.available() && Serial1.read()); // empty buffer again

    // load and configure the DMP
    Serial1.println(F("Initializing DMP..."));
    devStatus = mpu.dmpInitialize();

    // supply your own gyro offsets here, scaled for min sensitivity
    mpu.setXGyroOffset(-1);  //220    inc. will inc.
    mpu.setYGyroOffset(-24);  //76
    mpu.setZGyroOffset(51);  //-85
    mpu.setZAccelOffset(1788); // increasing number will increase 16384

    // make sure it worked (returns 0 if so)
    if (devStatus == 0) {
        // turn on the DMP, now that it's ready
        Serial1.println(F("Enabling DMP..."));
        mpu.setDMPEnabled(true);

        // enable Arduino interrupt detection
        Serial1.println(F("Enabling interrupt detection (Arduino Due external interrupt 2)..."));
        attachInterrupt(2, dmpDataReady, RISING);
        mpuIntStatus = mpu.getIntStatus();

        // set our DMP Ready flag so the main loop() function knows it's okay to use it
        Serial1.println(F("DMP ready! Waiting for first interrupt..."));
        dmpReady = true;

        // get expected DMP packet size for later comparison
        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)
        Serial1.print(F("DMP Initialization failed (code "));
        Serial1.print(devStatus);
        Serial1.println(F(")"));
    }

    // configure LED for output
    pinMode(LED_PIN, OUTPUT);
    
    delay(5000);
   
attachInterrupt(52, rpm_fun, FALLING);
Timer.getAvailable().attachInterrupt(firstHandler).start(1000000);  // from DueTimer library
delay(50);
}

void firstHandler(){        // Software interrupt every one second.
  Serial1.println("int");
  flag = true;
}
void rpm_fun()      // Hardware interrupt to count external pulses.
{
 count = count + 1;
}
// ================================================================
// ===                    MAIN PROGRAM LOOP                     ===
// ================================================================

void loop() {
    // if programming failed, don't try to do anything
    if (!dmpReady) return;

    // wait for MPU interrupt or extra packet(s) available
    while (!mpuInterrupt && fifoCount < packetSize) {
        // 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();

    // check for overflow (this should never happen unless our code is too inefficient)
    if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
        // reset so we can continue cleanly
        mpu.resetFIFO();
        Serial1.println(F("FIFO overflow!"));

    // otherwise, check for DMP data ready interrupt (this should happen frequently)
    } else if (mpuIntStatus & 0x02) {
        // wait for correct available data length, should be a VERY short wait
        while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

        // read a packet from FIFO
        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;


        #ifdef OUTPUT_READABLE_REALACCEL
            // display real acceleration, adjusted to remove gravity
            // -----Get data from MPU-6050-----

            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetAccel(&aa, fifoBuffer);
            mpu.dmpGetGravity(&gravity, &q);
            mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
            mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
            
            sample = -(aaReal.x);
            //Dummy = (abs(sample)>80) ? ((double)sample/16384)*9.81 : 0;
            Dummy = (double)sample/16384 * 9.81;
            t_s = (((double)micros() - timer)/1000000);
            
            
            // -----Get data from Arduino UNO-----
            Wire1.requestFrom(2, 1);    // request 1 byte from slave device #2
              while(Wire1.available())    // slave may send less than requested
             { 
                sped = Wire1.read(); // receive a byte as character
               vel_data = ( 0.85 *((double)(sped)* 0.216)) + (0.15 * X[1]);
               // Serial1.println(sped);         // print the character
              
            }

            //--------Prediction-------
            X[0] = X[0];  // Velocity
            X[1] = X[1];  // Acceleration
            
            P[0] = Q[0];
            P[1] = t_s * Q[1];
            
            //-------Kalman Gain--------
            S[0] = P[0] + R[0];
            S[1] = P[1] + R[1]; 
           
            K[0] = P[0]/S[0];
            K[1] = P[1]/S[1];
            
            //--------Update-------------
            
            //vel = vel + t_s * Dummy;
            
            
            Y[0] = vel_data - X[0];
            Y[1] = Dummy - X[1];  // Y[1] = Dummy - X[1];
            
            
            X[0] = X[0] + (K[0]*Y[0]);
            X[1] = X[1] + (K[1]*Y[1]);
            
            P[0] = P[0] - (K[0]*P[0]);
            P[1] = P[1] - (K[1]*P[1]);  
                        
            timer = micros();
            
        #endif

        // blink LED to indicate activity
        blinkState = !blinkState;
        digitalWrite(LED_PIN, blinkState);
    }   
}

Link to comment
Share on other sites

  • 1 year later...

Hi Anqurarora,

 

I also had problems with fifo overflow and randomly hanging loops.

Taking another mpu6050 sensor solved my problem totaly.

 

Wenn sensor, once in a while, is wired wrong but not totally fried yet, smaller pieces inside the sensor could be more or less damaged? Can't think of another reason.

 

Conclusion: trie a fresh sensor.

 

Succes,

 

skpprRlf.

Link to comment
Share on other sites

Join the conversation

You can post now and register later. If you have an account, sign in now to post with your account.

Guest
Reply to this topic...

×   Pasted as rich text.   Paste as plain text instead

  Only 75 emoji are allowed.

×   Your link has been automatically embedded.   Display as a link instead

×   Your previous content has been restored.   Clear editor

×   You cannot paste images directly. Upload or insert images from URL.

Loading...
 Share

×
×
  • Create New...