Jump to content
I2Cdevlib Forums


  • Posts

  • Joined

  • Last visited

  • Days Won


Posts posted by hatt0002

  1. Ok I have managed to solve the pitch and roll range issue. The pitch ranges from 1 to 90 and from 90 to zero. Also from 0 to -90 and from -90 to 0. I have noted that the transition at (89) to (90) to (89) and (-89) to (-90) to (-89) takes place when the gravity component in the z direction (gravity[2]) changes from positive to negative. At this instant, I am negating the value of the pitch/roll and subtracting a PI.  In this way, I am getting a 0-180 range for pitch and roll. Furthermore, If you want a 0-360 range, just subtract a 2PI when the current pitch/roll is greater than 0. this will give a range from 0 to -360. Take the absolute value. Here is my code.

    //calculating Pitch1
                if(gravity[2]>0) //range from 0 to 90 and from 0 to -90
                  ypr[1] = atan(gravity[0] / sqrt(gravity[1]*gravity[1] + gravity[2]*gravity[2]));
                else if(gravity[2]<0) //to get range from 90 to 180 and ftom -90 to -180
                  ypr[1] =  - (PI)-atan(gravity[0] / sqrt(gravity[1]*gravity[1] + gravity[2]*gravity[2]));
                  if(ypr[1] > 0) //to get range from 0 to -360
                    ypr[1] = -2*PI + ypr[1];
                  ypr[1] = abs(ypr[1]);
                //calculating Roll1
                  ypr[2] = atan(gravity[1] / sqrt(gravity[0]*gravity[0] + gravity[2]*gravity[2]));
                else if(gravity[2]<0)
                  ypr[2] = -PI - atan(gravity[1] / sqrt(gravity[0]*gravity[0] + gravity[2]*gravity[2]));
                  if(ypr[2] > 0) 
                    ypr[2] = -2*PI + ypr[2];
                  ypr[2] = abs(ypr[2]);

    Now my problem is the yaw is affected by the roll and pitch. I mean that when I change the roll without changing yaw, yaw still changes. For example on a 45 degree roll, the yaw changes by 10 degrees. Is this normal? Maybe Yaw is dependent on gravity component which I have to remove? If this is normal, is there a way to make yaw, pitch and roll independent on each other?
    Can someone please help since I need this issue to be solved as soon as possible.

    Thanks, Hatt

  2. Hi friends,

    I have managed to read from two mpu6050 on DMP. Both sensors are fitted on a piece of board and are perfectly levelled. 
    My problem is that when I tilt the sensors such that I increase/decrease Roll, the Yaw is changed, even when the sensors are not rotated on the z axis. Is it normal that rolling affects yawing? Furthermore, the change in Yaw for both sensors is not the same, and there will be a difference in yaw of 8 deg between the two sensors for a 45deg rolling.

    Thanks in advance,

  3. I have tried something similar to your code earlier and I have noticed the velocity drift. In my latest code, I have the actual acceleration which is the acceleration minus gravity component rounded to 1 decimal place using nfc() multiplied by 9.81 to get real acceleration as follows;

    String AccelX2i = nfc(AccelX2, 1); //This rounds up the acceleration to 1 decimal place and therefore I will always get the same absolute value during acceleration and decelleration, thus perfect cancellation
    gravity2i[0] = nfc(gravity2[0], 1); //similar to previous line
    ActualAccelX2=(float(AccelX2i)-float(gravity2i[0]))*9.81; //this gives me the real acceleration

    By these three lines of code, I know I am reducing the accuracy of acceleration, but I am also eliminating the drift.


    Then I am sampling the ActualAccelX2 data and store it in an array whose size increases with time. Also I am creating another array for the velocity;

    float [] AccX = new float[time]; //array for the current acceleration after sampling
    float [] vXn2 = new float[time]; //array for the current velocity
    if(millis() % 1 == 0) //sampling at 1KHz
            for(int n = 1; n<time; n++)
              AccX2[n] = ActualAccelX2; //filling the array with the current accel
              vXn2[n] = vXn2[n-1] + (AccX2[n])*0.001; //vX2[n] is the current velocity, vX2[n-1] is the previous velocity and AccX2[n] is the current accel

    This returns exactly what I need, but I am not sure if it is giving the correct values or not. If it is correct, then the drift is eliminated since the velocity always converges to zero when the sensor is stationary and increases as I start moving the sensor.

    Thanks, Hatt.

  4. Hi Kuba, Thankyou for your reply. I have done the following. I am saving the current acceleration, current velocity and previous velocity in arrays as follows;


    float [] AccX = new float[time]; //array for the current acceleration after sampling
    float [] AccY = new float[time];
    float [] AccZ = new float[time];

    float [] vXn = new float[time]; //array for the current velocity after integration
    float [] vYn = new float[time];
    float [] vZn = new float[time];

    float AccXn; //float for the current acceleration
    float AccYn;
    float AccZn;

    float v2XR; //float for the resulting velocity
    float v2YR;
    float v2ZR;

    float v2XP; //float for the previous velocity
    float v2YP;
    float v2ZP;

    float time = millis();
    if(millis() % 1 == 0) //sampling at 1KHz
    for(int n = 1; n<time; n++) //filling the arrays
    AccX[n] = ActualAccelX2; //filling an array with current values of acceleration after sampling
    AccY[n] = ActualAccelY2;
    AccZ[n] = ActualAccelZ2;

    AccXn = AccX[n]; //saving the current vaue of the acceleration array as float for the integration
    AccYn = AccY[n];
    AccZn = AccZ[n];

    vXn[n] = vXn[n-1] + (AccX[n])*0.001; //integration
    v2XR = (vXn[n]); //saving the current resultant velocity as a float
    u2XP = vXn[n-1]; //saving the previous velocity as a float

    vYn[n] = vYn[n-1] + (AccY[n])*0.001;
    v2YR = (vYn[n]);
    u2YP = vYn[n-1];

    vZn[n] = vZn[n-1] + (AccZ[n])*0.001;
    v2ZR = (vZn[n]);
    u2ZP = vZn[n-1];


    This returns exactly what I want. When I move the sensor, acceleration increases and so the velocity. When I stop moving the motor, velocity decreases to zero exactly. However, after I saw the video you posted, I am confused whether I am taking the good approach or not. The results returned looks good after doing some reverse engineering with a calculator (shown in the attached screenshot). Please can you verify whether I am right or wrong? 




    Thanks in advance, Hatt

  5. Hi, 

    I am currently doing a project in which I need to capture the acceleration of a pivoted rotating wooden beam. From the acceleration, I need to determine the velocity. 

    I am using Jeff's teapot sketch with the mpu6050 and I have managed to also include the acceleration in the teapotPacket to print acceleration in processing. I know that velocity is the first integral of acceleration. Such integral can be expressed as:
    v = u + AT, where v is the current velocity, u is the velocity before, A is the acceleration and T is a time interval.

    This is my code for the velocities in the three directions with sampling time of 1ms:


     vX = uX + (AX)*0.001; //vX = current velocity in the X direction, uX = velocity in the previous iteration, AX = acceleration in the x direc  uX = vX;                      //setting the current velocity as the previous velocity
     vY = uY + (AY)*0.001;
     uY = vY;
     vZ = uZ + (AZ)*0.001;
     uZ = vZ;
    If I move the sensor say in the x direction, acceleration in the x direction increases and when I stop the sensor, acceleration becomes negative and converges to zero, as expected. The velocity seems to be calculated and printed right when increasing from zero to a particular value. Now my problem is, that when I stop moving the sensor, velocity does not go back to zero when the sensor is stationary, but it decreases by a small amount only (say it stops decreasing at 0.5m/s. This gets worse when the sensor is moved faster. 

    It seems like the acceleration is read more than the deceleration (decreasing acceleration) and therefore, less negative values for acceleration are captured. 
    Can anybody help me?

    Thanks, Hatt
  6. Thanks again for your reply. I tried your code, but I had difficulties in the processing code, and I ended up getting wrong output. 

    My problem now is that in these lines;

    I am not getting a zero. I mean when I said before that I have managed to get the acceleration as 0, 0, 1g when the sensor is flat, I was actually getting 0.001g, 0.015g, 0.985g. The gravity component matches exactly in the X and Y direction, and the output is a nice, round zero. However, the Z component of gravity does not match exactly with AccelZ and I end up with an output of 0.01g which is equal to (0.01*9.81 = 0.1m/s^2). Is this difference due to defects in the sensor? or maybe I am doing something wrong?

    Thanks, Hatt
  7. Thanks for your quick reply. I think that I have achieved what I was looking for in a different way. What I did is;


    Arduino Code:



    #ifdef OUTPUT_TEAPOT


                      // display quaternion values in InvenSense TeaPot demo format:
                      TeapotPacket[2] = fifoBuffer[0];
                      TeapotPacket[3] = fifoBuffer[1];
                      TeapotPacket[4] = fifoBuffer[4];
                      TeapotPacket[5] = fifoBuffer[5];
                      TeapotPacket[6] = fifoBuffer[8];
                      TeapotPacket[7] = fifoBuffer[9];
                      TeapotPacket[8] = fifoBuffer[12];
                      TeapotPacket[9] = fifoBuffer[13];
                      // Added ACCEL
                      TeapotPacket[10] = fifoBuffer[28];
                      TeapotPacket[11] = fifoBuffer[29];
                      TeapotPacket[12] = fifoBuffer[32];
                      TeapotPacket[13] = fifoBuffer[33];
                      TeapotPacket[14] = fifoBuffer[36];
                      TeapotPacket[15] = fifoBuffer[37];
                      Serial.write(TeapotPacket, 20);
                      TeapotPacket[17]++; // packetCount, loops at 0xFF on purpose
    Processing Code:
    In serialEvent()...
                gravity[0] = 2 * (q[1]*q[3] - q[0]*q[2]);
                gravity[1] = 2 * (q[0]*q[1] + q[2]*q[3]);
                gravity[2] = q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3];
                   accel[0] = ((TeapotPacket[10] << 8) | TeapotPacket[11]);
                   accel[1] = ((TeapotPacket[12] << 8) | TeapotPacket[13]);
                   accel[2] = ((TeapotPacket[14] << 8) | TeapotPacket[15]);
                   if (accel[0] > 32767) accel[0] -= 65536;
                   if (accel[1] > 32767) accel[1] -= 65536;
                   if (accel[2] > 32767) accel[2] -= 65536; 
    Then in the draw()...
    Now the output ActualAccel is similar to what I am looking for.. Can you tell me if I am doing it the correct way or not?
    Thanks, Hatt
  8. Hi Kuba, thanks for your reply. However, I want acceleration to be displayed in processing. I have modified the arduino code and the processing code (something similar to georglue321's above code. I think that I am getting the correct values similar to the attached diagram (accelX, accelY, accelZ). 





    Also, when I tilt the sensor 45degrees in three axis, I get something like; 0.5g, 0.5g and 0.5g (XYZ).

    Now, I want that all accelerations are zero when the mpu is stationary, that is, I do not want the gravity component in my readings. I want the values of acceleration to only change when I move the mpu and back to zero when I stop it. Also, it must read zeroes when the mpu is moved at constant velocity. Do you understand what I mean?


  9. Hi Jeff,

    I have managed to obtain 0, 0, 1g in the processing code when the mpu6050 is flat using the code above. Can you tell me how can I obtain values without the gravity component? I want the readings being all zero when the sensor is stationary, regardless the orientation. The output must change accordingly when the sensor is moved, then return to zero when the sensor is stationary again. Please help me as I am stuck on this issue.

    Many thanks in advance.


  10. Hi all, 

    I am a student and I am using Jeff's excellent library for my school project. I am using i2cdevlib with arduino and processing and have managed to read data from the sensor successfully using the teapot demo. The problem that I am facing is that the dmp needs some time for auto calibration, and hence the yaw drifts for about 15-20 seconds on every start up. However this does not affect me so much. My problem is that the sensor will be installed on a boat, and therefore, it will never be static due to waves, wind etc. This led to the dmp remain in auto calibration process and never stops yaw drifting. So my question is, is there a way to still use the mpu6050 with the i2cdevlib and the calibration period completes while the sensor is moving due to the waves movements? Or maybe can I eliminate this auto calibration process?
    Please help me since I am running out of time, and have no ideas how to solve this problem.
    Thanks in advance

  11. Hi Jeff,
    Firstly, I would like to thank you for the nice library 12cdevlib. I am using your library with MPU6050 (GY521) for my school project involving orientation of a wooden beam.. Using your library, I have managed to determine the yaw, pitch and roll of the beam using one mpu6050 and displaying the beam using the teapot demo on processing. Now I need to add another beam, meaning that I need another MPU6050. Please can you tell me how can I modify your code to get two teapot outputs from two separate mpu6050 (eg. teapotPacket1 and teapotPacket2) so that I can display the two beams in processing. I only require the arduino uno sketch, I can manage alone for the processing sketch. Also, please tell me how to connect the two mpu6050 with the arduino uno board. (I am connect the two MPUs in parrallel except that AD0 of one MPU is 3.3V and the other floating)


    I am first trying to output readable yaw, pitch and roll and FIFO overflow error is occurring at the serial monitor.


    Thanks in advance




    #include "I2Cdev.h"
    #include "MPU6050_6Axis_MotionApps20.h"
        #include "Wire.h"
    MPU6050 mpu1(0x69);
    MPU6050 mpu2(0x68);
    //#define OUTPUT_READABLE_QUATERNION //for actual quaternion components in a [w, x, y, z] format
    //#define OUTPUT_READABLE_EULER //for Euler angles in degrees
    #define OUTPUT_READABLE_YAWPITCHROLL //for yaw, pitch and roll in degrees
    //#define OUTPUT_READABLE_REALACCEL //for acceleration components with gravity removed
    //#define OUTPUT_READABLE_WORLDACCEL //for acceleration components with gravity removed and adjusted for the world frame reference
    //#define OUTPUT_Oar //for Oar demonstation in processing.org
    #define LED_PIN 13 
    bool blinkState = false;
    // MPU control/status vars
    bool dmpReady1 = false;  // set true if DMP init was successful
    uint8_t mpuIntStatus1;   // holds actual interrupt status byte from MPU
    uint8_t devStatus1;      // return status after each device operation (0 = success, !0 = error)
    uint16_t packetSize1;    // expected DMP packet size (default is 42 bytes)
    uint16_t fifoCount1;     // count of all bytes currently in FIFO
    uint8_t fifoBuffer1[64]; // FIFO storage buffer
    // MPU control/status vars
    bool dmpReady2 = false;  // set true if DMP init was successful
    uint8_t mpuIntStatus2;   // holds actual interrupt status byte from MPU
    uint8_t devStatus2;      // return status after each device operation (0 = success, !0 = error)
    uint16_t packetSize2;    // expected DMP packet size (default is 42 bytes)
    uint16_t fifoCount2;     // count of all bytes currently in FIFO
    uint8_t fifoBuffer2[64]; // FIFO storage buffer
    // orientation/motion vars
    Quaternion q1;           // [w, x, y, z]         quaternion container
    VectorInt16 aa1;         // [x, y, z]            accel sensor measurements
    VectorInt16 aaReal1;     // [x, y, z]            gravity-free accel sensor measurements
    VectorInt16 aaWorld1;    // [x, y, z]            world-frame accel sensor measurements
    VectorFloat gravity1;    // [x, y, z]            gravity vector
    float euler1[3];         // [psi, theta, phi]    Euler angle container
    float ypr1[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector
    // orientation/motion vars
    Quaternion q2;           // [w, x, y, z]         quaternion container
    VectorInt16 aa2;         // [x, y, z]            accel sensor measurements
    VectorInt16 aaReal2;     // [x, y, z]            gravity-free accel sensor measurements
    VectorInt16 aaWorld2;    // [x, y, z]            world-frame accel sensor measurements
    VectorFloat gravity2;    // [x, y, z]            gravity vector
    float euler2[3];         // [psi, theta, phi]    Euler angle container
    float ypr2[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector
    // packet structure for InvenSense Oar demo
    uint8_t OarPacket1[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };
    uint8_t OarPacket2[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };
    //interrupt detection routine
    volatile bool mpuInterrupt1 = false;     // indicates whether MPU interrupt pin has gone high
    void dmpDataReady1() 
        mpuInterrupt1 = true;
    //interrupt detection routine
    volatile bool mpuInterrupt2 = false;     // indicates whether MPU interrupt pin has gone high
    void dmpDataReady2() 
        mpuInterrupt2 = true;
    //initial setup
    void setup() 
        // join I2C bus (I2Cdev library doesn't do this automatically)
            TWBR = 12; // 400kHz I2C clock (200kHz if CPU is 8MHz)
            Fastwire::setup(400, true);
        // initialize serial communication
        while (!Serial); 
        // initialize device
        Serial.println(F("Initializing I2C devices..."));
        // verify connection
        Serial.println(F("Testing device connections..."));
        Serial.println(mpu1.testConnection() ? F("MPU6050 #1 connection successful") : F("MPU6050 #1 connection failed"));
        Serial.println(mpu2.testConnection() ? F("MPU6050 #2 connection successful") : F("MPU6050 #2 connection failed"));
        // wait for ready
        Serial.println(F("\nSend any character to begin DMP programming and demo: "));
        while (Serial.available() && Serial.read()); // empty buffer
        while (!Serial.available());                 // wait for data
        while (Serial.available() && Serial.read()); // empty buffer again
        // load and configure the DMP
        Serial.println(F("Initializing DMP #1 ..."));
        devStatus1 = mpu1.dmpInitialize();
        devStatus2 = mpu2.dmpInitialize();
        // make sure it worked (returns 0 if so)
        if (devStatus1 == 0 || devStatus2 == 0) {
            // turn on the DMP, now that it's ready
            Serial.println(F("Enabling DMP ..."));
            // enable Arduino interrupt detection
            Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
            attachInterrupt(0, dmpDataReady1, RISING);
            mpuIntStatus1 = mpu1.getIntStatus();
          attachInterrupt(1, dmpDataReady2, RISING);
            mpuIntStatus2 = mpu2.getIntStatus();
            // set our DMP Ready flag so the main loop() function knows it's okay to use it
            Serial.println(F("DMP ready! Waiting for first interrupt..."));
            dmpReady1 = true;
            dmpReady2 = true;
            // get expected DMP packet size for later comparison
             packetSize1 = mpu1.dmpGetFIFOPacketSize();
             packetSize2 = mpu2.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 "));
        // configure LED for output
        pinMode(LED_PIN, OUTPUT);
    //main program loop
    void loop() {
        // if programming failed, don't try to do anything
        if (!dmpReady1) return;
        else if (!dmpReady2) return;
        // wait for MPU interrupt or extra packet(s) available
        while (!mpuInterrupt1 && fifoCount1 < packetSize1 && !mpuInterrupt2 && fifoCount2 < packetSize2) {}
        // reset interrupt flag and get INT_STATUS byte
        mpuInterrupt1 = false;
        mpuIntStatus1 = mpu1.getIntStatus();
        mpuInterrupt2 = false;
        mpuIntStatus2 = mpu2.getIntStatus();
        // get current FIFO count
        fifoCount1 = mpu1.getFIFOCount();
        fifoCount2 = mpu2.getFIFOCount();
        // check for overflow (this should never happen unless our code is too inefficient)
        if ((mpuIntStatus1 & 0x10) || fifoCount1 == 1024 ) {
            // reset so we can continue cleanly
            Serial.println(F("FIFO #1 overflow!"));
        // otherwise, check for DMP data ready interrupt (this should happen frequently)
         else if ((mpuIntStatus2 & 0x10) || fifoCount2 == 1024 ) {
            // reset so we can continue cleanly
            Serial.println(F("FIFO #2 overflow!"));
        // otherwise, check for DMP data ready interrupt (this should happen frequently)
        else if (mpuIntStatus1 & 0x02 && mpuIntStatus2 & 0x02) 
            // wait for correct available data length, should be a VERY short wait
            while (fifoCount1 < packetSize1 && fifoCount2 < packetSize2 ) 
            fifoCount1 = mpu1.getFIFOCount();
            fifoCount2 = mpu2.getFIFOCount();
            // read a packet from FIFO
            mpu1.getFIFOBytes(fifoBuffer1, packetSize1);
            mpu2.getFIFOBytes(fifoBuffer2, packetSize2);
            // track FIFO count here in case there is > 1 packet available
            // (this lets us immediately read more without waiting for an interrupt)
            fifoCount1 -= packetSize1;
             fifoCount2 -= packetSize2;
                // display Euler angles in degrees
                mpu1.dmpGetQuaternion(&q1, fifoBuffer1);
                mpu1.dmpGetGravity(&gravity1, &q1);
                mpu1.dmpGetYawPitchRoll(ypr1, &q1, &gravity1);
                Serial.print(ypr1[0] * 180/M_PI);
                Serial.print(ypr1[1] * 180/M_PI);
                Serial.println(ypr1[2] * 180/M_PI);
                // display Euler angles in degrees
                mpu2.dmpGetQuaternion(&q2, fifoBuffer2);
                mpu2.dmpGetGravity(&gravity2, &q2);
                mpu2.dmpGetYawPitchRoll(ypr2, &q2, &gravity2);
                Serial.print(ypr2[0] * 180/M_PI);
                Serial.print(ypr2[1] * 180/M_PI);
                Serial.println(ypr2[2] * 180/M_PI);
            #ifdef OUTPUT_Oar
                // display quaternion values in InvenSense Oar demo format:
                OarPacket1[2] = fifoBuffer1[0];
                OarPacket1[3] = fifoBuffer1[1];
                OarPacket1[4] = fifoBuffer1[4];
                OarPacket1[5] = fifoBuffer1[5];
                OarPacket1[6] = fifoBuffer1[8];
                OarPacket1[7] = fifoBuffer1[9];
                OarPacket1[8] = fifoBuffer1[12];
                OarPacket1[9] = fifoBuffer1[13];
                Serial.write(OarPacket1, 14);
                OarPacket1[11]++; // packetCount, loops at 0xFF on purpose
                 // display quaternion values in InvenSense Oar demo format:
                OarPacket2[2] = fifoBuffer2[0];
                OarPacket2[3] = fifoBuffer2[1];
                OarPacket2[4] = fifoBuffer2[4];
                OarPacket2[5] = fifoBuffer2[5];
                OarPacket2[6] = fifoBuffer2[8];
                OarPacket2[7] = fifoBuffer2[9];
                OarPacket2[8] = fifoBuffer2[12];
                OarPacket2[9] = fifoBuffer2[13];
                Serial.write(OarPacket2, 14);
                OarPacket2[11]++; // packetCount, loops at 0xFF on purpose
            // blink LED to indicate activity
            blinkState = !blinkState;
            digitalWrite(LED_PIN, blinkState);
  • Create New...