Jump to content
I2Cdevlib Forums

anqurarora

Members
  • Posts

    7
  • Joined

  • Last visited

  • Days Won

    2

anqurarora last won the day on August 10 2015

anqurarora had the most liked content!

anqurarora's Achievements

Newbie

Newbie (1/14)

4

Reputation

  1. Hi, the reason fifo is getting overflow is because the interrupt data rate of DMP is too high. To change the data rate you have to do the following, In line 305 of file “MPU6050_6axis_MotionApps20.h” the very last entry which is “0x01” decides the data rate as per the equation below, Data rate = (200 / (1+ value) ); Change this to 0x04, which will give you data rate of 40Hz and will work fine. Good luck.
  2. 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); } }
  3. Hi, copied is the working code. i could not attach it as a file. if you want .ino conatct me at anqur2004@gmail.com #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; double vlast; double kmp; double aax; float thr; double dtt; double RC; double alpha; uint32_t timer = 0; uint32_t timer1 = 0; uint32_t dt = 0; int16_t ax, ay, az; int16_t gx, gy, gz; // ================================================================ // === 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(); //TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz) #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE Fastwire::setup(400, true); #endif Serial1.begin(115200); 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(-35); //220 mpu.setYGyroOffset(-50); //76 mpu.setZGyroOffset(75); //-85 mpu.setZAccelOffset(1830); // 1688 factory default for my test chip // make sure it worked (returns 0 if so) if (devStatus == 0) { // turn on the DMP, now that it's ready 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); vel = 0; timer = micros(); vlast = 0; //Serial1.println(mpu.getFullScaleGyroRange()); //Serial1.println(mpu.getFullScaleAccelRange()); //mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_250); //mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_4); //Serial1.println(mpu.getFullScaleGyroRange()); //Serial1.println(mpu.getFullScaleAccelRange()); //Serial1.println(mpu.getDLPFMode()); //Serial1.println(mpu.getDHPFMode()); mpu.setDLPFMode(MPU6050_DLPF_BW_98); //Serial1.println(mpu.getDLPFMode()); } // ================================================================ // === 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 mpu.dmpGetQuaternion(&q, fifoBuffer); mpu.dmpGetAccel(&aa, fifoBuffer); mpu.dmpGetGravity(&gravity, &q); mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity); Serial1.print("areal\t"); Serial1.print(aaReal.x); Serial1.print(" "); Serial1.print(aaReal.y); Serial1.print(" "); Serial1.println(aaReal.z); //Serial1.print(" / "); #endif #ifdef OUTPUT_READABLE_WORLDACCEL // display initial world-frame acceleration, adjusted to remove gravity // and rotated based on known orientation from quaternion mpu.dmpGetQuaternion(&q, fifoBuffer); mpu.dmpGetAccel(&aa, fifoBuffer); mpu.dmpGetGravity(&gravity, &q); mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity); mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q); mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); thr = sqrt(pow((double)aaWorld.x/16384,2) + pow((double)aaWorld.y/16384,2) + pow((double)aaWorld.z/16384,2)); //aax = ((double) aaWorld.x * 0.70) + (aax*(0.30)); //thr = (double)aaWorld.x; // Print Result //---------------------------- // if(thr < 0.1 && (vlast < 0.1 && vlast > -0.1)) // { // vel=0; // // kmp = 0; // } // //dt = ((micros() - timer)); // if((aaWorld.x > 25 || aaWorld.x < -25 ) && (thr >= 0.009)) // { // //vel = (vel + (((double)aax / 16384) * ((double)(micros() - timer) / 1000000))); // vel = (vel + ((((double)aaWorld.x/16384) * 9.81) * ((double)(micros() - timer) / 1000000))); // // //kmp = vel * 3.6; // } // vlast = vel; // if(thr < 0.3 && (vlast > 0.1 || vlast < -0.1)) // { // vel = vlast; // } // else vel=0; //timer1 = (double)(micros() - timer1); //timer = micros(); // blinkState1 = !blinkState1; // digitalWrite(LED_PIN1, blinkState); //Serial1.print("areal\t"); Serial1.print("aworld\t"); Serial1.print(aaWorld.x); Serial1.print(" "); //Serial1.print(((double)aaWorld.x/16384)*9.81,4); //Serial1.print(" "); Serial1.print(aaWorld.y); Serial1.print(" "); Serial1.println(aaWorld.z); //Serial1.print(" "); //Serial1.print(vel,4); //Serial1.print(" "); //Serial1.println(thr,4); //Serial1.print(" "); //Serial1.println(dt); Serial1.print("Raw "); Serial1.print(ax); Serial1.print("\t"); Serial1.print(ay); Serial1.print("\t"); Serial1.println(az); Serial1.print("Gravity "); Serial1.print(gravity.x); Serial1.print(" "); Serial1.print(gravity.y);Serial1.print(" "); Serial1.println(gravity.z); Serial1.print("Mannual Commpensated "); Serial1.print(ax - (gravity.x*16384)); Serial1.print(" "); Serial1.print(ay - (gravity.y*16384)); Serial1.print(" "); Serial1.println(az - (gravity.z*16384)); //Serial1.print(gy); Serial1.print("\t"); //Serial1.println(gz);//Serial1.print("\t"); #endif // blink LED to indicate activity blinkState = !blinkState; digitalWrite(LED_PIN, blinkState); } }
  4. Hi Jeff, I tried 'MPU6050_raw' on uno and it worked fine (even MPU_DMP6 worked fine) but not on the due board. it gives me error "MPU6050 connection failed " and zeor values for all the sensor data. i connected 'SCL1 and SDA1' (i think that's the default for due). would you please help me out. Thank you.
  5. Hi jeff, Thanks for quick reply. I will try to run it on the due board by commenting that line (as i am expecting it to deleiver in 1-2 days) and hopefully it will work... what if i want i2c at 400khz? is there any other way. also my project is to calculate vehicle velocity using MPU_6050. i am trying to access 'gravity caompesated acceleration' and use it with the kalman library and finally controlling a motor using PID library. do you think it is feasible and DUE is powerfull enough to do all the work (cause i need atleast 20 samples/min). and i think PID and KALMAN library uses the same timers..will this be a problem? Thank you.
  6. Hi, I tried MPU_DMP6.ino and it compiles without error for UNO (haven’t tested on real board), but I am using DUE board and the sketch won’t compile for the arduino due board. Could you please tell me why I am getting this error, will be helpful. MPU6050_DMP6.ino: In function 'void setup()': MPU6050_DMP6:165: error: 'TWBR' was not declared in this scope If I put it as a comment then it compiles without error. but still dont know if it will actually work or not. Thank you.
×
×
  • Create New...