hatt0002

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 if(gravity[2]>0) { 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

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, Hatt

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.

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

Hi, Are there any news about this topic? I have the same problem and I need the pitch and roll to have a range 0 to 180 and 0 to -180. Did someone managed to do the conversion? Thanks, Hatt

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

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; ActualAccelX=AccelX-gravity[0]; ActualAccelY=AccelY-gravity[1]; ActualAccelZ=AccelZ-gravity[2]; 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

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 #endif 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()... AccelX=accel[0]/8192; AccelY=accel[1]/8192; AccelZ=accel[2]/8192; ActualAccelX=AccelX-gravity[0]; ActualAccelY=AccelY-gravity[1]; ActualAccelZ=AccelZ-gravity[2]; text(......)...etc 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

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? Thankyou, Hatt

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. Hatt

Hi immunity, could you tell me how you have managed to calculate linear acceleration in SI units (m/s^2)?

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

Thankyou so much. You saved me alot of time

Hi, does anyone knows how to get acceleration in m/s^2 from the i2cdevlib OUTPUT_READABLE_WORLDACCEL? Further more, how to convert acceleration to velocity in m/s? Thanks in advance

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" #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE #include "Wire.h" #endif 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) #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE Wire.begin(); TWBR = 12; // 400kHz I2C clock (200kHz if CPU is 8MHz) #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE Fastwire::setup(400, true); #endif // initialize serial communication Serial.begin(9600); while (!Serial); // initialize device Serial.println(F("Initializing I2C devices...")); mpu1.initialize(); mpu2.initialize(); // 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 ...")); mpu1.setDMPEnabled(true); mpu2.setDMPEnabled(true); // 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 ")); Serial.print(devStatus1); Serial.print(devStatus2); Serial.println(F(")")); } // 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 mpu1.resetFIFO(); 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 mpu2.resetFIFO(); 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; #ifdef OUTPUT_READABLE_YAWPITCHROLL // display Euler angles in degrees mpu1.dmpGetQuaternion(&q1, fifoBuffer1); mpu1.dmpGetGravity(&gravity1, &q1); mpu1.dmpGetYawPitchRoll(ypr1, &q1, &gravity1); Serial.print("ypr1\t"); Serial.print(ypr1[0] * 180/M_PI); Serial.print("\t"); Serial.print(ypr1[1] * 180/M_PI); Serial.print("\t"); 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\t"); Serial.print(ypr2[0] * 180/M_PI); Serial.print("\t"); Serial.print(ypr2[1] * 180/M_PI); Serial.print("\t"); Serial.println(ypr2[2] * 180/M_PI); #endif #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 #endif // blink LED to indicate activity blinkState = !blinkState; digitalWrite(LED_PIN, blinkState); } }