Jump to content
I2Cdevlib Forums
georgelue321

Modifying Example DMP Code To Also Print Acceleration Data

Recommended Posts

Hello!

I have modified the DMP example code to also print the acceleration readings from the FIFO buffer.
 
Arduino Modified 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
 
 
The Processing Code:
 
                   q[0] = ((teapotPacket[2] << 8) | teapotPacket[3]) / 16384.0f;
                    q[1] = ((teapotPacket[4] << 8) | teapotPacket[5]) / 16384.0f;
                    q[2] = ((teapotPacket[6] << 8) | teapotPacket[7]) / 16384.0f;
                    q[3] = ((teapotPacket[8] << 8) | teapotPacket[9]) / 16384.0f;
                    accel[0] = ((teapotPacket[10] << 8) | teapotPacket[11]);
                    accel[1] = ((teapotPacket[12] << 8) | teapotPacket[13]);
                    accel[2] = ((teapotPacket[14] << 8) | teapotPacket[15]);
 
 
However, the raw acceleration readings in Processing are always
65522.0 64328.0 7593.0
 
What exactly am I doing wrong?
Thanks.
-George

 

Share this post


Link to post
Share on other sites

The processing code also aligns itself on a 14-byte boundary before reading in the data. Make sure you have edited that portion accordingly. Alternatively you could completely remove the portion so that it starts reading data as soon it encounters the '$' symbol.

Share this post


Link to post
Share on other sites

Hi George,

 

You probably aren't doing anything wrong, actually. The raw accel data comes in a 16-bit container with data stored in 2's complement form. This means that the variable range is [-32768, +32767]. Negative numbers are represented by anything >= 32768. Compilers and often map this 2-byte memory spaces right onto another 16-bit signed variable container and work with negative numbers automatically, but if you want to compute it by hand, you would do the following after the Processing code you listed:

if (accel[0] > 32767) accel[0] -= 65536;
if (accel[1] > 32767) accel[1] -= 65536;
if (accel[2] > 32767) accel[2] -= 65536;

This will offset all of the values to the correct range. The MPU actually only uses [-16384, +16383] for its raw data (not sure why), which means you should never see any unshifted numbers between 16384 and 49151.

Share this post


Link to post
Share on other sites

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

Share this post


Link to post
Share on other sites

Hi,

I think that you want to get exaclty the same thing that is implemented in the example file

 

i2cdevlib-master\Arduino\MPU6050\Examples\MPU6050_DMP6\MPU6050_DMP6.ino

 

You have to uncomment one of this definitions:

 

// uncomment "OUTPUT_READABLE_REALACCEL" if you want to see acceleration
// components with gravity removed. This acceleration reference frame is
// not compensated for orientation, so +X is always +X according to the
// sensor, just without the effects of gravity. If you want acceleration
// compensated for orientation, us OUTPUT_READABLE_WORLDACCEL instead.
//#define OUTPUT_READABLE_REALACCEL

// uncomment "OUTPUT_READABLE_WORLDACCEL" if you want to see acceleration
// components with gravity removed and adjusted for the world frame of
// reference (yaw is relative to initial orientation, since no magnetometer
// is present in this case). Could be quite handy in some cases.
//#define OUTPUT_READABLE_WORLDACCEL

and you will get acceleration without gravity. Check lines 324-352 to see what is exaclty happening.

Share this post


Link to post
Share on other sites

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

 

 

accelerometer-ADXL-335-11.jpg

 

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

Share this post


Link to post
Share on other sites

One of these two

 

OUTPUT_READABLE_REALACCEL

OUTPUT_READABLE_WORLDACCEL

is exactly what you want, because it should return 0 values when MPU is stationary. The only difference should be axes:

MPU.jpg

If MPU6050 is stationary then it will return 0 (almost 0) in all three axes. You just have to use the code from lines 324-352 (or copy/modify it).

 

If you want to have the teapot example and these values you can modify the code like this:

 

// packet structure for InvenSense teapot demo
uint8_t teapotPacket[17] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0, 0, 0, 0x00, 0x00, '\r', '\n' };

      

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

 

            //This is copy paste from 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);
            teapotPacket[10] = aaWorld.x;
            teapotPacket[11] = aaWorld.y;
            teapotPacket[12] = aaWorld.z;

 

            Serial.write(teapotPacket, 17);
            teapotPacket[14]++; // packetCount, loops at 0xFF on purpose
        #endif

 

Now you should be able to use world acceleration data inside teapot demo. Just be careful because teapot array structure has been slightly modified.

Share this post


Link to post
Share on other sites

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

Share this post


Link to post
Share on other sites

It seems ok and you are getting OUTPUT_READABLE_REALACCEL.

You are doing almost the same that is done by this functions:

 

            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetAccel(&aa, fifoBuffer);
            mpu.dmpGetGravity(&gravity, &q);
            mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);

 

Your code contains the same logic, but written in slightly different order (but if you look closely you will see the same multiplications, shift operations, subtractions etc).

I just think that my suggestions would make the code much easier to read.

Share this post


Link to post
Share on other sites

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

Share this post


Link to post
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...

×
×
  • Create New...