I2Cdevlib Forums

# hatt0002

Members

15

4

## Posts posted by hatt0002

1. ### roll and pitch angles ranges

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?

Thanks, Hatt

2. ### roll affecting yaw?

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.

Hatt

3. ### velocity from acceleration

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. ### velocity from acceleration

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?

5. ### roll and pitch angles ranges

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

6. ### velocity from acceleration

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

7. ### Modifying Example DMP Code To Also Print Acceleration Data

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
8. ### Modifying Example DMP Code To Also Print Acceleration Data

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];

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
9. ### Modifying Example DMP Code To Also Print Acceleration Data

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

10. ### Modifying Example DMP Code To Also Print Acceleration Data

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.

Hatt

11. ### Help at Calculating linear acceleration

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

12. ### mpu6050 auto calibration period

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.

13. ### acceleration in m/s^2

Thankyou so much. You saved me alot of time

14. ### acceleration in m/s^2

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?

15. ### two mpu6050

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.

#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

// 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
{
mpuInterrupt1 = true;
}
//interrupt detection routine
volatile bool mpuInterrupt2 = false;     // indicates whether MPU interrupt pin has gone high
{
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"));

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)..."));
mpuIntStatus1 = mpu1.getIntStatus();
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..."));

// 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

// 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;

// 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
}
}
×