Search the Community

All documentation on how to use the DMP is now freely available from InvenSense here. This means you can either rewrite this library so it works with all aspects of the DMP, or you can use the documentation and sample code written by InvenSense, to make the DMP work without this library. This information has been released for over two years... I hope you finally get to fully appreciate the DMP, instead using this half working library made by hacking an evaluation board. (To the creator: Your work has been to great help for many people, and now finally we get to use the DMP fully. Hadn't this been released, I would have been very grateful for your work.)

Hello everyone. I'm quite new to Arduino and i2c so please bear with me even if my questions may have an obvious answer from your point of view. Looking at the example of 6 axis MPU 6050 I'm trying to wrap my head around some programming decision that was made, specifically: 1) Why do we run while loop while (!mpuInterrupt && fifoCount < packetSize) { inside the main Arduino loop() instead of just simply running single interrupt flag and fifoCount check each iteration of the loop(), is that to catch new data package as absolutely soon as possible? 2) Why do we use interrupt at all, can't we just check getFIFOCount() and if > packetSize - retrieve data. I've modified the example to do just that and it seems that data is still fine, nothing borked e.t.c. I might be missing the whole point of using interrupts here, please clarify this for me if possible. Thanks in advance!

#ifdef OUTPUT_READABLE_WORLDACCEL #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); Serial.print("aworld\t"); Serial.print(aaWorld.x); Serial.print("\t"); Serial.print(aaWorld.y); Serial.print("\t"); Serial.println(aaWorld.z); #endif I want to ask that what is the unit of above aaWorld values ? I understand that if i need to get the acceleration without gravity i need to use these values , but the values that we get from these are in m/s^2? or do i need to do something acc_in_ms2=(aaWorld.x*9.8)/16384.0 ? or something else. my offset are as follows mpu.setXGyroOffset(220); mpu.setYGyroOffset(76); mpu.setZGyroOffset(6); mpu.setZAccelOffset(1262); and my output is as follows without doing any changes in code provided after 10-15 sec (the sensor is horizontal) aworld 0 28 60 aworld -1 29 40 aworld 5 29 33 aworld 6 24 20 aworld 2 23 22 aworld -4 13 26 aworld -6 11 28 aworld -10 11 30 aworld -15 5 38 aworld -7 -1 36 aworld 7 -4 37 aworld 7 0 33 aworld 4 5 29 aworld 0 2 30 aworld -3 -7 35 seems coreect all the values converging to zeros.. but when i move the sensor in one direction , i get totally unexpected values.. aworld -415 -306 304 aworld -505 -277 318 aworld -577 -270 399 aworld -587 -207 265 aworld -572 -166 63 aworld -589 -200 -28 aworld -631 -258 -7 aworld -638 -292 30 aworld -637 -334 29 aworld -647 -404 -23 aworld -640 -483 -75 aworld -611 -542 -72 aworld -633 -503 -24 aworld -723 -344 17 aworld -816 -125 36 aworld -908 72 1 aworld -988 241 -45 aworld -1015 378 -68 I am moving the sesor only to and fro in one direction but i am getting these values Why so? please help if you have any information /knowledge about this... I want acceleration mithout gravity affect in m/s^2

Hi everybody. I am trying to use the dmp of the mpu 6050. I am using an arduino nano and windows 10. Also I am using the sketch "MPU6050_DMP6.ino" with almost no changes. When I run the sketch the dmp initializes correctly, but then, when it shows the quaternion´s components it only shows zeros, besides the fifo is overflown once evey 14 printed values. Similar problems occur when i try to get euler angles or yaw, pitch and roll. Any idea of what may be happening? or what i could do? Thanks a lot! The code i am using is posted in https://pastebin.com/HHL0d3WW

I'm running the DMP example for Arduino. Upon startup the yaw value wanders for about ten seconds before stabilizing at ~38°. Pitch and roll are stable from the start, but after the ten seconds everybody is basically perfect. This is confusing to me because of the way sensor fusion works. In a fused sensor the acceleration vector is used to cancel drift in the x and y gyro readings, and a magnetometer does the same for roll around z. The MPU-6050 doesn't have a magnetometer, so why does the yaw stabilize? I know something's not quite right with the yaw; it drifts for a while when pitch and roll are steady from the start. However, from looking at the erratic drift in the raw gyro rates, I don't see a reason why the yaw would ever stabilize. I realize that I'm kind of asking "How does DMP work" and that nobody really knows. But maybe someone has an idea? Anyway, thanks for the work you've done, Jeff. Your praises shall be sung to the farthest reaches of the interwebs, followed by lusty cursings of InvenSense. But maybe a kind word every once in a while.

Hi All, I have 5 different MPU6050 on one mechanic. I'm using Jeff's DMP6 code and I am especially interested in pitch and roll values from "OUTPUT_READABLE_YAWPITCHROLL". I already calibrated All 5 MPU with "MPU6050_calibration.ino" All sensors gives me "0 degree" when they placed in horizontal position, with package letters facing up. Also I'm getting "90 degree"(almost) they placed vertical. When i place sensors in forexample 45 degree all of them shows different angles between 43-47, this error follows me in different angles. i have no error on vertical and horizontal planes but there is a non lineer error occurs between 0-90 degree and its not same for all. supply voltage and ambient temperature is same for all and i read sensors from 5 different comport. I suspect for calibration process and try a lot of time to calibrate each other but nothing changed. DMP6 code works perfect, there is no freeze or connection problem but this difference makes me crazy :)

Hello, My name is Robert, I'm a 3'rd year computer science student i i'm working on a bracelet using the MPU 6050 and PSOC 4 BLE chip. I am in need of a bit of help as i have very little knowledge on programming hardware like this. The problem i have is that the chip doesn't produce data. So a standard run works like this: 1. it initializes ok, err is 0 2. fifo count and packet count = 42 dec, 2a hex but isDataReady is false first pass in the for loop so it skips the if 3. second pass in the for isDataReady is true so it processes the data (no new data in the fifo meanwhile, still 42) 4. reads the data from fifo into the buffer and outputs a quaternion, now fifo is empty 5. 3'rd pass fifo count still 0 and isDataReady is true 7. it gets trapped on line 144 at the while loop for reading the fifo count as 0 forever And i have no idea why. Some of the code for the dmp i translated myself from c++ but i'm pretty sure it's ok because i didn't modify the logic behind. P.S. i have some random variables that i use for debugging around there and i know it's dirty but i'm just testing main.c

Hi, I'm trying to flash the DMP image to an ICM-20948 to use the built-in sensor fusion. I've adapted my source code to use the code from the "Embedded MotionDriver (eMD) ICM-20948 v.1.0 for Nucleo Board" functions. I'm using nRF52832 to talk to ICM-20948 through I2C. My problem is twofold: 1) The transfer of the DMP firmware (dmp3a) sometimes fails. The ICM-20948 NACKs a byte (strangely it always seems to be a packet in memory bank 19 or 30), and then I have to power-cycle the chip to get an ACK from any register read/write. 2) In the cases where the DMP firmware transfer is successful, the verification routine fails. The bytes that my device reads back to compare to the firmware bytes are just all zeroes. For reference, this is the firmware upload routine from the MotionDriver source code: int inv_icm20948_firmware_load(struct inv_icm20948 * s, const unsigned char *data_start, unsigned short size_start, unsigned short load_addr) { int write_size; int result; unsigned short memaddr; const unsigned char *data; unsigned short size; unsigned char data_cmp[INV_MAX_SERIAL_READ]; int flag = 0; if(s->base_state.firmware_loaded) return 0; // Write DMP memory data = data_start; size = size_start; memaddr = load_addr; while (size > 0) { write_size = min(size, INV_MAX_SERIAL_WRITE); if ((memaddr & 0xff) + write_size > 0x100) { // Moved across a bank write_size = (memaddr & 0xff) + write_size - 0x100; } result = inv_icm20948_write_mems(s, memaddr, write_size, (unsigned char *)data); if (result) return result; data += write_size; size -= write_size; memaddr += write_size; } // Verify DMP memory data = data_start; size = size_start; memaddr = load_addr; while (size > 0) { write_size = min(size, INV_MAX_SERIAL_READ); if ((memaddr & 0xff) + write_size > 0x100) { // Moved across a bank write_size = (memaddr & 0xff) + write_size - 0x100; } result = inv_icm20948_read_mems(s, memaddr, write_size, data_cmp); if (result) flag++; // Error, DMP not written correctly if (memcmp(data_cmp, data, write_size)) return -1; data += write_size; size -= write_size; memaddr += write_size; } #if defined(WIN32) //if(!flag) // inv_log("DMP Firmware was updated successfully..\r\n"); #endif return 0; } I've looked at the I2C lines on a scope, and the bytes are successfully written to the ICM-20948, but somehow they are not making it to the DMP memory. I've attached a screenshot of the NACK event (third byte). I assume the following constants are suitable for the ICM-20948: #define DMP_LOAD_START 0x90 #define REG_MEM_START_ADDR (BANK_0 | 0x7C) #define REG_MEM_R_W (BANK_0 | 0x7D) #define REG_MEM_BANK_SEL (BANK_0 | 0x7E)

Hello, I am attaching an Arduino example sketch to show how to read a magnetometer (HMC5883L) while MPU's DMP is active. This is only useful if you have your magnetometer attached to MPU's I2C internal aux lines, like many 10DOF boards do, and you want to use DMP. If you can, I believe it is always best to connect your magnetometer to your main I2C lines directly and you won't need this sketch. This sketch does NOT fuse DMP + magnetometer. You will have to do it by yourself in your host processor. I would only try to fuse magnetometer if it is going to be used outdoors, as electromagnetic interferences caused by electronics indoor make it quite difficult. Besides, the DMP's yaw measure is very good if you don't need much precision or long running times, in these cases you should fuse magnetometer + DMP. Please read sketch comments, as many things are explained there. Remember to put your MPU's address (0x68 or 0x69) and to set your interruption pin. Please give feedback, wheter it works or you have trouble with it. If people find it useful maybe Jeff can add it to the library once it is bugfree. Thank you MPU6050_DMP_mag.zip

Hello, I have been trying to find a solution for this issue for a while now. Any info/suggestion/pointing in the right direction would be of great help. I am working on a project where I extract quaternions to deduce orientation using MPU6050 (DMP). I use the Arduino and libraries by Jeff Rowberg for calibration and reading from sensor. I have had a prototype working with MPU6050 mounted the standard way(gravity along Z) and it works fine. I then built a custom PCB with MPU6050 chip, which is then assembled to my project. The difference being, PCB is assembled such that the gravity is not along Z, it is along X. It doesn't work as I expected (like the initial prototype). When I turn on the device, I do some initial calculations to reference it (world to body coordinates conversion). This doesn't behave as I expect. My questions are, 1. Does the DMP or library assume that the gravity is always aligned along Z by default? 2. What steps should I follow to tell the DMP/Sensor that the gravity is aligned along X and not Z? I am not an electronics guy and haven't worked extensively with sensors. So, if what I ask is stupid, please forgive me. Please do share your thoughts. Many Thanks. Kind Regards, Kailash

To the I2CDev community, I am having trouble understanding the units of the MPU6050::dmpGetGyro() functions from the MPU6050_6Axis_MotionApps20.h file in the MPU6050 library (I can't find any documentation for the functions). It seems that these functions do not output raw integers like the MPU6050::getMotion6() function in the main library. In fact, I have figured out empirically that the dmpGetGyro(VectorInt16 *v, const uint8_t* packet) function outputs integers in degrees/second when the gyro range is the default of +/- 2000 deg/sec, but I would like a precision greater than 1 degree/second, and I cannot find out how to convert the 16 bit dmpGetGyro(int16_t *data, const uint8_t* packet) values to deg/second. Furthermore, if I change the gyro range (and therefore also the sensitivity), the readings change, and I need to know how to convert my readings at each sensitivity. My question is similar to this unanswered one from stack overflow. Thank you so much for your response.

I'm using the 9150 and the axes for how how it's mounted on my PCB are different from the default Invensense mounting. The documentation says to use the orientation matrix (or Axis Transformation Matrix) to perform this transformation. My problem is that making any changes at all to the gyro_pdata.orientation matrix introduces significant drift. If the matrix is left as the default orientation it works fine, there is almost no drift in my 3D model. static struct platform_data_s gyro_pdata = { .orientation = { 1, 0, 0, 0, 1, 0, 0, 0, 1} }; But say if I want to swap and negate x and y, my 3D model will start to drift significantly. static struct platform_data_s gyro_pdata = { .orientation = { 0, -1, 0, -1, 0, 0, 0, 0, 1} }; The gyro_pdata.orientation matrix is set as the orientation for both the accelerometer and gyro. The DMP documentation says that the orientation matrix is the only thing that needs to be changed when changing the chip mounting orientation. I was wondering if anyone else has experience this issue and how they fixed it? Thanks.

Hi everyone, I'm working on a project with an Arduino Teensy 3.2 and the MPU-6050. My method to sample the IMU is based on the "MPU6050_DMP6.ino" example in the library. I modified the program and made it into a function that returns three or four values (angles/accelerations or quaternion), instead of printing them to the serial port. Not much of a change. Anyway - the program works well for a minute or two (sometimes even five). It prints out the angles to the serial monitor and they all seem to make sense. But, after said minute or two (or five) - it all goes to... crap. Instead of "roll 29 pitch 0.1 yaw 1.4" (just an example) I'm suddenly seeing "roll -150 pitch nan yaw 300". Then the outputs go crazy and change values frantically for another minute or so - after which they seem to stop at a certain set of angles (roll -180, pitch 0, yaw -180). And by "stop", I mean they don't change even when I move the MPU around. EDIT: I just noticed that it goes back to normal after a long while. Like after ten minutes, the values would be normal again for one to two minutes. Then it goes mental once more. Does anyone have an idea what the problem might be? I thought it might be FIFO overflow but I don't see any messages in my log. Any help would be greatly appreciated, as always.

Hi all, I'm using MPU 9150 9-axis sensor with arduino mega board, i want to run a task in DMP itself in background continuously to reduce the power consumption,is it possible to run our code with DMP, if possible how can i achieve that,Please help me..Thanks in advance..

My question is simple. Is it possible to use the DMP output without using the interruptions method? My goal is also simple. I'm trying to read 3D acceleration without gravity by taking advantage of the actual dedicated processor for that task. I took the example that comes with the Jeff's library and got it complied and executed just perfectly. The thing is I don't want the interruption system because half I don't completely understand the code, half I need to take samples at a fixed rate. I'm pretty sure the DMP is way faster than my sampling rate, and even though it isn't, skipping a couple of random samples is not an issue in my project. So it would be awesome if I could do just something like this (my imagination coming out now) void loop() { mpu.dmpGetQuaternion(&q, fifoBuffer); mpu.dmpGetAccel(&aa, fifoBuffer); mpu.dmpGetGravity(&gravity, &q); mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity); Serial.print(aaReal.x); // awfully printed just to make my point Serial.print(aaReal.y); Serial.println(aaReal.z); delay(100); } to get acceleration without gravity using only a few functions. Note: I already did the corresponding calibration to get my offsets.

Is it possible to get Q15 format from MPU6050 or MPU6000? I think it can be reached by dividing output data by 32768 by DMP and shift 14 bits right side to left side ? I need data in -1 to +1 range .

Hello everybody, First of all, thanks a lot for the Library, this saved me a lot of precious time! I'm here to ask for a little help : I'm using an Arduino Due (using arduino 1.5.5-r2 software) to read the data from my MPU6050, using the DMP. I read the angles using the same code as in the example code for the DMP and I tried a lot of different configurations, without any change. I noticed that reading the values takes my approximately 6ms. This is a little bit long for me, but the interesting thing is that the sketch spends 5ms of the 6 in the call of this function: // read a packet from FIFO mpu.getFIFOBytes(fifoBuffer, packetSize); I don't understand how this can be so long. This is just reading registers and the packetSize is only 42... So here are my questions. Is this normal with an MPU6050 or is there effectively something strange ? Does anybody have any idea why this is happening? When this function is called, the data should be ready, so why do I loose so much time ? I hope somebody will be able to help me with this, because otherwise I'll have to read the raw data and process them by myself. So thanks in advance for your answers!

Hey all, I have an MPU-6050 connected to a simple board, with only SCL, SDA and voltage/GND outputs. Meaning, no INT pin to connect to my Arduino (I am using Teensy 3.2). Now I don't know a whole lot about these things, but going by the sketches I've seen so far (well, basically the one sketch by the almighty Jeff) - it is required to use the INT pin in order to access the DMP. Is that just the way the sketch is built, or am I screwed by this board not having the INT output? Because I would really love to get Euler angles or quaternions but I've only seen these calculations done reliably through the DMP. Any help would be appreciated.

Hi everybody... I've been working on a project recently and i need to measure the roll and pitch angles of a moving platform, but the problem is that the value of these angles starts from 0 then reaches 90, then starts to decrease again to reach 0 (positive or negative 90 degrees of course) ... what i need to do is to have this value in the range of zero to 180 (positive or negative) not from zero to 90 as it is now ... do anyone have any idea??

Hello, I found a bug in the MPU6050 library. When using I2CDEV_IMPLEMENTATION=I2CDEV_BUILTIN_FASTWIRE, the dmpInialize functions hangs here: ... DEBUG_PRINTLN(F("Waiting for FIFO count > 2...")); while ((fifoCount = getFIFOCount()) < 3); ... (in file MPU6050_6Axis_MotionApps20.h) Could someone guide me how to solve this bug?

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); } }

well I dont have any Problem with the arduino sketch and libraries. I can read all the Information from the sensor. My question is related to the dmp integreted in the unit. Als is that mentioned in the Header file "MPU6050_6Axis_MotionApps20.h" many data will be written to different Registers at power on. if i have unterstand the Routine "dmpinitialize" at first a block of Memory will be written (the first Register in which the first Byte will be written is the Register 0x6F") and then configuration data will be written and finally "dmpupdate" data. Is there anyone who can affirm that the first Register in which the first block Memory data will be written at power on is 0x68? and i would appreciate when i got also in which adress the other data will be written? ( i have tried to find it out but until now i cannot get it) i have tried to Display the Content of some Registers from the Serial Monitor provided by arduino IDE. I have added a few line of fuctions from the library "wire" just after the line "devStatus = mpu.dmpInitialize();" My program dont do anything just initialize dmp. the Content of the Register are not equal to any of the series of data to write through the Routine "dmpinitiaze" Please help me to understand what is going!!!!! Ah why am i interested to These adress? I want to reprogramm in Assembler avr and i should These adress at first step known. I would appreciate any Help, even a Suggestion to read up to 10M Byte pdf document.

Hi lads, I managed to do the mpu6050 DMP initialization via a PIC24F device using my own straightforward IIC routines. The excercise was based on Jeff Rowberg's MPU6050_6Axis_MotionApps20.h document and the original I2C analysis spreadsheet produced I believe by Noah Zerkin, which I had downloaded back in 2011 i think it was. Many thanks to JR and NZ for their work. I read the 16 bit quaternion data from the FIFO and divide by 16384, to obtain the normalized quaternion (float data type) from which I extract the YPR angles using arctan2() functions for yaw and roll, and an arcsin() function to obtain pitch. The three calculations (2 x arctan2 and 1 x arcsin funcs) involve around 12836 instruction cycles which with my PIC24 running at 16 MIPS take 12836 * 1/16 us = 802.25us ~ .8 ms I have a few questions: 1) Does anybody know if the DMP can output the YPR angles directly as it does with Quat data? 2) What is the significance of the three OTP (one time programmable?) flags? I am referring to the OTP_BNK_VLD bit (bit 0) in registers XG_OFFS_TC (0x00),YG_OFFS_TC (0x01), ZG_OFFS_TC (0x02). I downloaded the MPU HW Offset Registers 1.1.pdf AppNote which makes it clear how to configure your own Gyro and Accel offset values but am not sure how it all ties with the OTP flags. I left the offset registers as is and got quite good YPR angles. 3) Why 32 bit Gyro and Accel data in FIFO when the raw data is available as 16 bit values in the sensor registers? A couple of observations: a) Note that line 298: "0x00, 0x00, 0x00, 0x01, // SET INT_ENABLE at i=22, SPECIAL INSTRUCTION" in MPU6050_6Axis_MotionApps20.h contains 0x00 in the payload field when it should be 0x01. Not a big deal but can mess up your indexing if you use a single function to do the writes. Memory update 6 of 7 is actually a read and not a write. This is reflected in MPU6050_6Axis_MotionApps20.h and in the IIC analyzer file. Nevertheless in my procedure I just wrote the corresponding values as in the rest of the updates. It seems to work OK. Sorry if I posed questions that have been answered elsewhere. I have made an effort to read this and the Invensense forum. If anybody is interested I can make available my code and procedure in the "Other Platforms" section of the forum. Cheers

HELLO, can someone please help me in understanding this code ? Atleast the part that how do you initialize the DMP or get the DMP values ? I want to write my own code and i am trying to refer to this code by jeff rowberg : or atleast point me to resources where i can understand how the reading and initialization of DMP works I have been trying this since weeks now and really need some help : #ifndef _MPU6050_6AXIS_MOTIONAPPS20_H_ #define _MPU6050_6AXIS_MOTIONAPPS20_H_ #include "I2Cdev.h" #include "helper_3dmath.h" // MotionApps 2.0 DMP implementation, built using the MPU-6050EVB evaluation board #define MPU6050_INCLUDE_DMP_MOTIONAPPS20 #include "MPU6050.h" // Tom Carpenter's conditional PROGMEM code // http://forum.arduino.cc/index.php?topic=129407.0 #ifndef __arm__ #include <avr/pgmspace.h> #else // Teensy 3.0 library conditional PROGMEM code from Paul Stoffregen #ifndef __PGMSPACE_H_ #define __PGMSPACE_H_ 1 #include <inttypes.h> #define PROGMEM #define PGM_P const char * #define PSTR(str) (str) #define F(x) x typedef void prog_void; typedef char prog_char; typedef unsigned char prog_uchar; typedef int8_t prog_int8_t; typedef uint8_t prog_uint8_t; typedef int16_t prog_int16_t; typedef uint16_t prog_uint16_t; typedef int32_t prog_int32_t; typedef uint32_t prog_uint32_t; #define strcpy_P(dest, src) strcpy((dest), (src)) #define strcat_P(dest, src) strcat((dest), (src)) #define strcmp_P(a, b) strcmp((a), () #define pgm_read_byte(addr) (*(const unsigned char *)(addr)) #define pgm_read_word(addr) (*(const unsigned short *)(addr)) #define pgm_read_dword(addr) (*(const unsigned long *)(addr)) #define pgm_read_float(addr) (*(const float *)(addr)) #define pgm_read_byte_near(addr) pgm_read_byte(addr) #define pgm_read_word_near(addr) pgm_read_word(addr) #define pgm_read_dword_near(addr) pgm_read_dword(addr) #define pgm_read_float_near(addr) pgm_read_float(addr) #define pgm_read_byte_far(addr) pgm_read_byte(addr) #define pgm_read_word_far(addr) pgm_read_word(addr) #define pgm_read_dword_far(addr) pgm_read_dword(addr) #define pgm_read_float_far(addr) pgm_read_float(addr) #endif #endif /* Source is from the InvenSense MotionApps v2 demo code. Original source is * unavailable, unless you happen to be amazing as decompiling binary by * hand (in which case, please contact me, and I'm totally serious). * * Also, I'd like to offer many, many thanks to Noah Zerkin for all of the * DMP reverse-engineering he did to help make this bit of wizardry * possible. */ // NOTE! Enabling DEBUG adds about 3.3kB to the flash program size. // Debug output is now working even on ATMega328P MCUs (e.g. Arduino Uno) // after moving string constants to flash memory storage using the F() // compiler macro (Arduino IDE 1.0+ required). //#define DEBUG #ifdef DEBUG #define DEBUG_PRINT(x) Serial.print(x) #define DEBUG_PRINTF(x, y) Serial.print(x, y) #define DEBUG_PRINTLN(x) Serial.println(x) #define DEBUG_PRINTLNF(x, y) Serial.println(x, y) #else #define DEBUG_PRINT(x) #define DEBUG_PRINTF(x, y) #define DEBUG_PRINTLN(x) #define DEBUG_PRINTLNF(x, y) #endif #define MPU6050_DMP_CODE_SIZE 1929 // dmpMemory[] #define MPU6050_DMP_CONFIG_SIZE 192 // dmpConfig[] #define MPU6050_DMP_UPDATES_SIZE 47 // dmpUpdates[] /* ================================================================================================ * | Default MotionApps v2.0 42-byte FIFO packet structure: | | | | [QUAT W][ ][QUAT X][ ][QUAT Y][ ][QUAT Z][ ][GYRO X][ ][GYRO Y][ ] | | 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 | | | | [GYRO Z][ ][ACC X ][ ][ACC Y ][ ][ACC Z ][ ][ ] | | 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 | * ================================================================================================ */ // this block of memory gets written to the MPU on start-up, and it seems // to be volatile memory, so it has to be done each time (it only takes ~1 // second though) const unsigned char dmpMemory[MPU6050_DMP_CODE_SIZE] PROGMEM = { // bank 0, 256 bytes 0xFB, 0x00, 0x00, 0x3E, 0x00, 0x0B, 0x00, 0x36, 0x00, 0x01, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00, 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0xFA, 0x80, 0x00, 0x0B, 0x12, 0x82, 0x00, 0x01, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x28, 0x00, 0x00, 0xFF, 0xFF, 0x45, 0x81, 0xFF, 0xFF, 0xFA, 0x72, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 0xE8, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x7F, 0xFF, 0xFF, 0xFE, 0x80, 0x01, 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3E, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xCA, 0xE3, 0x09, 0x3E, 0x80, 0x00, 0x00, 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00, 0x41, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x0B, 0x2A, 0x00, 0x00, 0x16, 0x55, 0x00, 0x00, 0x21, 0x82, 0xFD, 0x87, 0x26, 0x50, 0xFD, 0x80, 0x00, 0x00, 0x00, 0x1F, 0x00, 0x00, 0x00, 0x05, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x6F, 0x00, 0x02, 0x65, 0x32, 0x00, 0x00, 0x5E, 0xC0, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xFB, 0x8C, 0x6F, 0x5D, 0xFD, 0x5D, 0x08, 0xD9, 0x00, 0x7C, 0x73, 0x3B, 0x00, 0x6C, 0x12, 0xCC, 0x32, 0x00, 0x13, 0x9D, 0x32, 0x00, 0xD0, 0xD6, 0x32, 0x00, 0x08, 0x00, 0x40, 0x00, 0x01, 0xF4, 0xFF, 0xE6, 0x80, 0x79, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0xD0, 0xD6, 0x00, 0x00, 0x27, 0x10, // bank 1, 256 bytes 0xFB, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0xFA, 0x36, 0xFF, 0xBC, 0x30, 0x8E, 0x00, 0x05, 0xFB, 0xF0, 0xFF, 0xD9, 0x5B, 0xC8, 0xFF, 0xD0, 0x9A, 0xBE, 0x00, 0x00, 0x10, 0xA9, 0xFF, 0xF4, 0x1E, 0xB2, 0x00, 0xCE, 0xBB, 0xF7, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x04, 0x00, 0x02, 0x00, 0x02, 0x02, 0x00, 0x00, 0x0C, 0xFF, 0xC2, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0xCF, 0x80, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 0x3F, 0x68, 0xB6, 0x79, 0x35, 0x28, 0xBC, 0xC6, 0x7E, 0xD1, 0x6C, 0x80, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0xB2, 0x6A, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3F, 0xF0, 0x00, 0x00, 0x00, 0x30, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x25, 0x4D, 0x00, 0x2F, 0x70, 0x6D, 0x00, 0x00, 0x05, 0xAE, 0x00, 0x0C, 0x02, 0xD0, // bank 2, 256 bytes 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x44, 0x00, 0x00, 0x00, 0x00, 0x0C, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x54, 0x00, 0x00, 0xFF, 0xEF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // bank 3, 256 bytes 0xD8, 0xDC, 0xBA, 0xA2, 0xF1, 0xDE, 0xB2, 0xB8, 0xB4, 0xA8, 0x81, 0x91, 0xF7, 0x4A, 0x90, 0x7F, 0x91, 0x6A, 0xF3, 0xF9, 0xDB, 0xA8, 0xF9, 0xB0, 0xBA, 0xA0, 0x80, 0xF2, 0xCE, 0x81, 0xF3, 0xC2, 0xF1, 0xC1, 0xF2, 0xC3, 0xF3, 0xCC, 0xA2, 0xB2, 0x80, 0xF1, 0xC6, 0xD8, 0x80, 0xBA, 0xA7, 0xDF, 0xDF, 0xDF, 0xF2, 0xA7, 0xC3, 0xCB, 0xC5, 0xB6, 0xF0, 0x87, 0xA2, 0x94, 0x24, 0x48, 0x70, 0x3C, 0x95, 0x40, 0x68, 0x34, 0x58, 0x9B, 0x78, 0xA2, 0xF1, 0x83, 0x92, 0x2D, 0x55, 0x7D, 0xD8, 0xB1, 0xB4, 0xB8, 0xA1, 0xD0, 0x91, 0x80, 0xF2, 0x70, 0xF3, 0x70, 0xF2, 0x7C, 0x80, 0xA8, 0xF1, 0x01, 0xB0, 0x98, 0x87, 0xD9, 0x43, 0xD8, 0x86, 0xC9, 0x88, 0xBA, 0xA1, 0xF2, 0x0E, 0xB8, 0x97, 0x80, 0xF1, 0xA9, 0xDF, 0xDF, 0xDF, 0xAA, 0xDF, 0xDF, 0xDF, 0xF2, 0xAA, 0xC5, 0xCD, 0xC7, 0xA9, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97, 0x97, 0xF1, 0xA9, 0x89, 0x26, 0x46, 0x66, 0xB0, 0xB4, 0xBA, 0x80, 0xAC, 0xDE, 0xF2, 0xCA, 0xF1, 0xB2, 0x8C, 0x02, 0xA9, 0xB6, 0x98, 0x00, 0x89, 0x0E, 0x16, 0x1E, 0xB8, 0xA9, 0xB4, 0x99, 0x2C, 0x54, 0x7C, 0xB0, 0x8A, 0xA8, 0x96, 0x36, 0x56, 0x76, 0xF1, 0xB9, 0xAF, 0xB4, 0xB0, 0x83, 0xC0, 0xB8, 0xA8, 0x97, 0x11, 0xB1, 0x8F, 0x98, 0xB9, 0xAF, 0xF0, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xF1, 0xA3, 0x29, 0x55, 0x7D, 0xAF, 0x83, 0xB5, 0x93, 0xAF, 0xF0, 0x00, 0x28, 0x50, 0xF1, 0xA3, 0x86, 0x9F, 0x61, 0xA6, 0xDA, 0xDE, 0xDF, 0xD9, 0xFA, 0xA3, 0x86, 0x96, 0xDB, 0x31, 0xA6, 0xD9, 0xF8, 0xDF, 0xBA, 0xA6, 0x8F, 0xC2, 0xC5, 0xC7, 0xB2, 0x8C, 0xC1, 0xB8, 0xA2, 0xDF, 0xDF, 0xDF, 0xA3, 0xDF, 0xDF, 0xDF, 0xD8, 0xD8, 0xF1, 0xB8, 0xA8, 0xB2, 0x86, // bank 4, 256 bytes 0xB4, 0x98, 0x0D, 0x35, 0x5D, 0xB8, 0xAA, 0x98, 0xB0, 0x87, 0x2D, 0x35, 0x3D, 0xB2, 0xB6, 0xBA, 0xAF, 0x8C, 0x96, 0x19, 0x8F, 0x9F, 0xA7, 0x0E, 0x16, 0x1E, 0xB4, 0x9A, 0xB8, 0xAA, 0x87, 0x2C, 0x54, 0x7C, 0xB9, 0xA3, 0xDE, 0xDF, 0xDF, 0xA3, 0xB1, 0x80, 0xF2, 0xC4, 0xCD, 0xC9, 0xF1, 0xB8, 0xA9, 0xB4, 0x99, 0x83, 0x0D, 0x35, 0x5D, 0x89, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0xB5, 0x93, 0xA3, 0x0E, 0x16, 0x1E, 0xA9, 0x2C, 0x54, 0x7C, 0xB8, 0xB4, 0xB0, 0xF1, 0x97, 0x83, 0xA8, 0x11, 0x84, 0xA5, 0x09, 0x98, 0xA3, 0x83, 0xF0, 0xDA, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xD8, 0xF1, 0xA5, 0x29, 0x55, 0x7D, 0xA5, 0x85, 0x95, 0x02, 0x1A, 0x2E, 0x3A, 0x56, 0x5A, 0x40, 0x48, 0xF9, 0xF3, 0xA3, 0xD9, 0xF8, 0xF0, 0x98, 0x83, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0x97, 0x82, 0xA8, 0xF1, 0x11, 0xF0, 0x98, 0xA2, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xDA, 0xF3, 0xDE, 0xD8, 0x83, 0xA5, 0x94, 0x01, 0xD9, 0xA3, 0x02, 0xF1, 0xA2, 0xC3, 0xC5, 0xC7, 0xD8, 0xF1, 0x84, 0x92, 0xA2, 0x4D, 0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9, 0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0x93, 0xA3, 0x4D, 0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9, 0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0xA8, 0x8A, 0x9A, 0xF0, 0x28, 0x50, 0x78, 0x9E, 0xF3, 0x88, 0x18, 0xF1, 0x9F, 0x1D, 0x98, 0xA8, 0xD9, 0x08, 0xD8, 0xC8, 0x9F, 0x12, 0x9E, 0xF3, 0x15, 0xA8, 0xDA, 0x12, 0x10, 0xD8, 0xF1, 0xAF, 0xC8, 0x97, 0x87, // bank 5, 256 bytes 0x34, 0xB5, 0xB9, 0x94, 0xA4, 0x21, 0xF3, 0xD9, 0x22, 0xD8, 0xF2, 0x2D, 0xF3, 0xD9, 0x2A, 0xD8, 0xF2, 0x35, 0xF3, 0xD9, 0x32, 0xD8, 0x81, 0xA4, 0x60, 0x60, 0x61, 0xD9, 0x61, 0xD8, 0x6C, 0x68, 0x69, 0xD9, 0x69, 0xD8, 0x74, 0x70, 0x71, 0xD9, 0x71, 0xD8, 0xB1, 0xA3, 0x84, 0x19, 0x3D, 0x5D, 0xA3, 0x83, 0x1A, 0x3E, 0x5E, 0x93, 0x10, 0x30, 0x81, 0x10, 0x11, 0xB8, 0xB0, 0xAF, 0x8F, 0x94, 0xF2, 0xDA, 0x3E, 0xD8, 0xB4, 0x9A, 0xA8, 0x87, 0x29, 0xDA, 0xF8, 0xD8, 0x87, 0x9A, 0x35, 0xDA, 0xF8, 0xD8, 0x87, 0x9A, 0x3D, 0xDA, 0xF8, 0xD8, 0xB1, 0xB9, 0xA4, 0x98, 0x85, 0x02, 0x2E, 0x56, 0xA5, 0x81, 0x00, 0x0C, 0x14, 0xA3, 0x97, 0xB0, 0x8A, 0xF1, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9, 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x84, 0x0D, 0xDA, 0x0E, 0xD8, 0xA3, 0x29, 0x83, 0xDA, 0x2C, 0x0E, 0xD8, 0xA3, 0x84, 0x49, 0x83, 0xDA, 0x2C, 0x4C, 0x0E, 0xD8, 0xB8, 0xB0, 0xA8, 0x8A, 0x9A, 0xF5, 0x20, 0xAA, 0xDA, 0xDF, 0xD8, 0xA8, 0x40, 0xAA, 0xD0, 0xDA, 0xDE, 0xD8, 0xA8, 0x60, 0xAA, 0xDA, 0xD0, 0xDF, 0xD8, 0xF1, 0x97, 0x86, 0xA8, 0x31, 0x9B, 0x06, 0x99, 0x07, 0xAB, 0x97, 0x28, 0x88, 0x9B, 0xF0, 0x0C, 0x20, 0x14, 0x40, 0xB8, 0xB0, 0xB4, 0xA8, 0x8C, 0x9C, 0xF0, 0x04, 0x28, 0x51, 0x79, 0x1D, 0x30, 0x14, 0x38, 0xB2, 0x82, 0xAB, 0xD0, 0x98, 0x2C, 0x50, 0x50, 0x78, 0x78, 0x9B, 0xF1, 0x1A, 0xB0, 0xF0, 0x8A, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x8B, 0x29, 0x51, 0x79, 0x8A, 0x24, 0x70, 0x59, 0x8B, 0x20, 0x58, 0x71, 0x8A, 0x44, 0x69, 0x38, 0x8B, 0x39, 0x40, 0x68, 0x8A, 0x64, 0x48, 0x31, 0x8B, 0x30, 0x49, 0x60, 0xA5, 0x88, 0x20, 0x09, 0x71, 0x58, 0x44, 0x68, // bank 6, 256 bytes 0x11, 0x39, 0x64, 0x49, 0x30, 0x19, 0xF1, 0xAC, 0x00, 0x2C, 0x54, 0x7C, 0xF0, 0x8C, 0xA8, 0x04, 0x28, 0x50, 0x78, 0xF1, 0x88, 0x97, 0x26, 0xA8, 0x59, 0x98, 0xAC, 0x8C, 0x02, 0x26, 0x46, 0x66, 0xF0, 0x89, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31, 0xA9, 0x88, 0x09, 0x20, 0x59, 0x70, 0xAB, 0x11, 0x38, 0x40, 0x69, 0xA8, 0x19, 0x31, 0x48, 0x60, 0x8C, 0xA8, 0x3C, 0x41, 0x5C, 0x20, 0x7C, 0x00, 0xF1, 0x87, 0x98, 0x19, 0x86, 0xA8, 0x6E, 0x76, 0x7E, 0xA9, 0x99, 0x88, 0x2D, 0x55, 0x7D, 0x9E, 0xB9, 0xA3, 0x8A, 0x22, 0x8A, 0x6E, 0x8A, 0x56, 0x8A, 0x5E, 0x9F, 0xB1, 0x83, 0x06, 0x26, 0x46, 0x66, 0x0E, 0x2E, 0x4E, 0x6E, 0x9D, 0xB8, 0xAD, 0x00, 0x2C, 0x54, 0x7C, 0xF2, 0xB1, 0x8C, 0xB4, 0x99, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0x81, 0x91, 0xAC, 0x38, 0xAD, 0x3A, 0xB5, 0x83, 0x91, 0xAC, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9, 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0x8C, 0x9D, 0xAE, 0x29, 0xD9, 0x04, 0xAE, 0xD8, 0x51, 0xD9, 0x04, 0xAE, 0xD8, 0x79, 0xD9, 0x04, 0xD8, 0x81, 0xF3, 0x9D, 0xAD, 0x00, 0x8D, 0xAE, 0x19, 0x81, 0xAD, 0xD9, 0x01, 0xD8, 0xF2, 0xAE, 0xDA, 0x26, 0xD8, 0x8E, 0x91, 0x29, 0x83, 0xA7, 0xD9, 0xAD, 0xAD, 0xAD, 0xAD, 0xF3, 0x2A, 0xD8, 0xD8, 0xF1, 0xB0, 0xAC, 0x89, 0x91, 0x3E, 0x5E, 0x76, 0xF3, 0xAC, 0x2E, 0x2E, 0xF1, 0xB1, 0x8C, 0x5A, 0x9C, 0xAC, 0x2C, 0x28, 0x28, 0x28, 0x9C, 0xAC, 0x30, 0x18, 0xA8, 0x98, 0x81, 0x28, 0x34, 0x3C, 0x97, 0x24, 0xA7, 0x28, 0x34, 0x3C, 0x9C, 0x24, 0xF2, 0xB0, 0x89, 0xAC, 0x91, 0x2C, 0x4C, 0x6C, 0x8A, 0x9B, 0x2D, 0xD9, 0xD8, 0xD8, 0x51, 0xD9, 0xD8, 0xD8, 0x79, // bank 7, 138 bytes (remainder) 0xD9, 0xD8, 0xD8, 0xF1, 0x9E, 0x88, 0xA3, 0x31, 0xDA, 0xD8, 0xD8, 0x91, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9, 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x83, 0x93, 0x35, 0x3D, 0x80, 0x25, 0xDA, 0xD8, 0xD8, 0x85, 0x69, 0xDA, 0xD8, 0xD8, 0xB4, 0x93, 0x81, 0xA3, 0x28, 0x34, 0x3C, 0xF3, 0xAB, 0x8B, 0xF8, 0xA3, 0x91, 0xB6, 0x09, 0xB4, 0xD9, 0xAB, 0xDE, 0xFA, 0xB0, 0x87, 0x9C, 0xB9, 0xA3, 0xDD, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x95, 0xF1, 0xA3, 0xA3, 0xA3, 0x9D, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0xF2, 0xA3, 0xB4, 0x90, 0x80, 0xF2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xB2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xB0, 0x87, 0xB5, 0x99, 0xF1, 0xA3, 0xA3, 0xA3, 0x98, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x97, 0xA3, 0xA3, 0xA3, 0xA3, 0xF3, 0x9B, 0xA3, 0xA3, 0xDC, 0xB9, 0xA7, 0xF1, 0x26, 0x26, 0x26, 0xD8, 0xD8, 0xFF }; // thanks to Noah Zerkin for piecing this stuff together! const unsigned char dmpConfig[MPU6050_DMP_CONFIG_SIZE] PROGMEM = { // BANK OFFSET LENGTH [DATA] 0x03, 0x7B, 0x03, 0x4C, 0xCD, 0x6C, // FCFG_1 inv_set_gyro_calibration 0x03, 0xAB, 0x03, 0x36, 0x56, 0x76, // FCFG_3 inv_set_gyro_calibration 0x00, 0x68, 0x04, 0x02, 0xCB, 0x47, 0xA2, // D_0_104 inv_set_gyro_calibration 0x02, 0x18, 0x04, 0x00, 0x05, 0x8B, 0xC1, // D_0_24 inv_set_gyro_calibration 0x01, 0x0C, 0x04, 0x00, 0x00, 0x00, 0x00, // D_1_152 inv_set_accel_calibration 0x03, 0x7F, 0x06, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97, // FCFG_2 inv_set_accel_calibration 0x03, 0x89, 0x03, 0x26, 0x46, 0x66, // FCFG_7 inv_set_accel_calibration 0x00, 0x6C, 0x02, 0x20, 0x00, // D_0_108 inv_set_accel_calibration 0x02, 0x40, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_00 inv_set_compass_calibration 0x02, 0x44, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_01 0x02, 0x48, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_02 0x02, 0x4C, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_10 0x02, 0x50, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_11 0x02, 0x54, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_12 0x02, 0x58, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_20 0x02, 0x5C, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_21 0x02, 0xBC, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_22 0x01, 0xEC, 0x04, 0x00, 0x00, 0x40, 0x00, // D_1_236 inv_apply_endian_accel 0x03, 0x7F, 0x06, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97, // FCFG_2 inv_set_mpu_sensors 0x04, 0x02, 0x03, 0x0D, 0x35, 0x5D, // CFG_MOTION_BIAS inv_turn_on_bias_from_no_motion 0x04, 0x09, 0x04, 0x87, 0x2D, 0x35, 0x3D, // FCFG_5 inv_set_bias_update 0x00, 0xA3, 0x01, 0x00, // D_0_163 inv_set_dead_zone // SPECIAL 0x01 = enable interrupts 0x00, 0x00, 0x00, 0x01, // SET INT_ENABLE at i=22, SPECIAL INSTRUCTION 0x07, 0x86, 0x01, 0xFE, // CFG_6 inv_set_fifo_interupt 0x07, 0x41, 0x05, 0xF1, 0x20, 0x28, 0x30, 0x38, // CFG_8 inv_send_quaternion 0x07, 0x7E, 0x01, 0x30, // CFG_16 inv_set_footer 0x07, 0x46, 0x01, 0x9A, // CFG_GYRO_SOURCE inv_send_gyro 0x07, 0x47, 0x04, 0xF1, 0x28, 0x30, 0x38, // CFG_9 inv_send_gyro -> inv_construct3_fifo 0x07, 0x6C, 0x04, 0xF1, 0x28, 0x30, 0x38, // CFG_12 inv_send_accel -> inv_construct3_fifo 0x02, 0x16, 0x02, 0x00, 0x01 // D_0_22 inv_set_fifo_rate // This very last 0x01 WAS a 0x09, which drops the FIFO rate down to 20 Hz. 0x07 is 25 Hz, // 0x01 is 100Hz. Going faster than 100Hz (0x00=200Hz) tends to result in very noisy data. // DMP output frequency is calculated easily using this equation: (200Hz / (1 + value)) // It is important to make sure the host processor can keep up with reading and processing // the FIFO output at the desired rate. Handling FIFO overflow cleanly is also a good idea. }; const unsigned char dmpUpdates[MPU6050_DMP_UPDATES_SIZE] PROGMEM = { 0x01, 0xB2, 0x02, 0xFF, 0xFF, 0x01, 0x90, 0x04, 0x09, 0x23, 0xA1, 0x35, 0x01, 0x6A, 0x02, 0x06, 0x00, 0x01, 0x60, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x60, 0x04, 0x40, 0x00, 0x00, 0x00, 0x01, 0x62, 0x02, 0x00, 0x00, 0x00, 0x60, 0x04, 0x00, 0x40, 0x00, 0x00 }; uint8_t MPU6050::dmpInitialize() { // reset device DEBUG_PRINTLN(F("\n\nResetting MPU6050...")); reset(); delay(30); // wait after reset // enable sleep mode and wake cycle /*Serial.println(F("Enabling sleep mode...")); setSleepEnabled(true); Serial.println(F("Enabling wake cycle...")); setWakeCycleEnabled(true);*/ // disable sleep mode DEBUG_PRINTLN(F("Disabling sleep mode...")); setSleepEnabled(false); // get MPU hardware revision DEBUG_PRINTLN(F("Selecting user bank 16...")); setMemoryBank(0x10, true, true); DEBUG_PRINTLN(F("Selecting memory byte 6...")); setMemoryStartAddress(0x06); DEBUG_PRINTLN(F("Checking hardware revision...")); uint8_t hwRevision = readMemoryByte(); DEBUG_PRINT(F("Revision @ user[16][6] = ")); DEBUG_PRINTLNF(hwRevision, HEX); DEBUG_PRINTLN(F("Resetting memory bank selection to 0...")); setMemoryBank(0, false, false); // check OTP bank valid DEBUG_PRINTLN(F("Reading OTP bank valid flag...")); uint8_t otpValid = getOTPBankValid(); DEBUG_PRINT(F("OTP bank is ")); DEBUG_PRINTLN(otpValid ? F("valid!") : F("invalid!")); // get X/Y/Z gyro offsets DEBUG_PRINTLN(F("Reading gyro offset TC values...")); int8_t xgOffsetTC = getXGyroOffsetTC(); int8_t ygOffsetTC = getYGyroOffsetTC(); int8_t zgOffsetTC = getZGyroOffsetTC(); DEBUG_PRINT(F("X gyro offset = ")); DEBUG_PRINTLN(xgOffset); DEBUG_PRINT(F("Y gyro offset = ")); DEBUG_PRINTLN(ygOffset); DEBUG_PRINT(F("Z gyro offset = ")); DEBUG_PRINTLN(zgOffset); // setup weird slave stuff (?) DEBUG_PRINTLN(F("Setting slave 0 address to 0x7F...")); setSlaveAddress(0, 0x7F); DEBUG_PRINTLN(F("Disabling I2C Master mode...")); setI2CMasterModeEnabled(false); DEBUG_PRINTLN(F("Setting slave 0 address to 0x68 (self)...")); setSlaveAddress(0, 0x68); DEBUG_PRINTLN(F("Resetting I2C Master control...")); resetI2CMaster(); delay(20); // load DMP code into memory banks DEBUG_PRINT(F("Writing DMP code to MPU memory banks (")); DEBUG_PRINT(MPU6050_DMP_CODE_SIZE); DEBUG_PRINTLN(F(" bytes)")); if (writeProgMemoryBlock(dmpMemory, MPU6050_DMP_CODE_SIZE)) { DEBUG_PRINTLN(F("Success! DMP code written and verified.")); // write DMP configuration DEBUG_PRINT(F("Writing DMP configuration to MPU memory banks (")); DEBUG_PRINT(MPU6050_DMP_CONFIG_SIZE); DEBUG_PRINTLN(F(" bytes in config def)")); if (writeProgDMPConfigurationSet(dmpConfig, MPU6050_DMP_CONFIG_SIZE)) { DEBUG_PRINTLN(F("Success! DMP configuration written and verified.")); DEBUG_PRINTLN(F("Setting clock source to Z Gyro...")); setClockSource(MPU6050_CLOCK_PLL_ZGYRO); DEBUG_PRINTLN(F("Setting DMP and FIFO_OFLOW interrupts enabled...")); setIntEnabled(0x12); DEBUG_PRINTLN(F("Setting sample rate to 200Hz...")); setRate(4); // 1khz / (1 + 4) = 200 Hz DEBUG_PRINTLN(F("Setting external frame sync to TEMP_OUT_L[0]...")); setExternalFrameSync(MPU6050_EXT_SYNC_TEMP_OUT_L); DEBUG_PRINTLN(F("Setting DLPF bandwidth to 42Hz...")); setDLPFMode(MPU6050_DLPF_BW_42); DEBUG_PRINTLN(F("Setting gyro sensitivity to +/- 2000 deg/sec...")); setFullScaleGyroRange(MPU6050_GYRO_FS_2000); DEBUG_PRINTLN(F("Setting DMP configuration bytes (function unknown)...")); setDMPConfig1(0x03); setDMPConfig2(0x00); DEBUG_PRINTLN(F("Clearing OTP Bank flag...")); setOTPBankValid(false); DEBUG_PRINTLN(F("Setting X/Y/Z gyro offset TCs to previous values...")); setXGyroOffsetTC(xgOffsetTC); setYGyroOffsetTC(ygOffsetTC); setZGyroOffsetTC(zgOffsetTC); //DEBUG_PRINTLN(F("Setting X/Y/Z gyro user offsets to zero...")); //setXGyroOffset(0); //setYGyroOffset(0); //setZGyroOffset(0); DEBUG_PRINTLN(F("Writing final memory update 1/7 (function unknown)...")); uint8_t dmpUpdate[16], j; uint16_t pos = 0; for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); DEBUG_PRINTLN(F("Writing final memory update 2/7 (function unknown)...")); for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); DEBUG_PRINTLN(F("Resetting FIFO...")); resetFIFO(); DEBUG_PRINTLN(F("Reading FIFO count...")); uint16_t fifoCount = getFIFOCount(); uint8_t fifoBuffer[128]; DEBUG_PRINT(F("Current FIFO count=")); DEBUG_PRINTLN(fifoCount); getFIFOBytes(fifoBuffer, fifoCount); DEBUG_PRINTLN(F("Setting motion detection threshold to 2...")); setMotionDetectionThreshold(2); DEBUG_PRINTLN(F("Setting zero-motion detection threshold to 156...")); setZeroMotionDetectionThreshold(156); DEBUG_PRINTLN(F("Setting motion detection duration to 80...")); setMotionDetectionDuration(80); DEBUG_PRINTLN(F("Setting zero-motion detection duration to 0...")); setZeroMotionDetectionDuration(0); DEBUG_PRINTLN(F("Resetting FIFO...")); resetFIFO(); DEBUG_PRINTLN(F("Enabling FIFO...")); setFIFOEnabled(true); DEBUG_PRINTLN(F("Enabling DMP...")); setDMPEnabled(true); DEBUG_PRINTLN(F("Resetting DMP...")); resetDMP(); DEBUG_PRINTLN(F("Writing final memory update 3/7 (function unknown)...")); for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); DEBUG_PRINTLN(F("Writing final memory update 4/7 (function unknown)...")); for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); DEBUG_PRINTLN(F("Writing final memory update 5/7 (function unknown)...")); for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); DEBUG_PRINTLN(F("Waiting for FIFO count > 2...")); while ((fifoCount = getFIFOCount()) < 3); DEBUG_PRINT(F("Current FIFO count=")); DEBUG_PRINTLN(fifoCount); DEBUG_PRINTLN(F("Reading FIFO data...")); getFIFOBytes(fifoBuffer, fifoCount); DEBUG_PRINTLN(F("Reading interrupt status...")); uint8_t mpuIntStatus = getIntStatus(); DEBUG_PRINT(F("Current interrupt status=")); DEBUG_PRINTLNF(mpuIntStatus, HEX); DEBUG_PRINTLN(F("Reading final memory update 6/7 (function unknown)...")); for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); readMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); DEBUG_PRINTLN(F("Waiting for FIFO count > 2...")); while ((fifoCount = getFIFOCount()) < 3); DEBUG_PRINT(F("Current FIFO count=")); DEBUG_PRINTLN(fifoCount); DEBUG_PRINTLN(F("Reading FIFO data...")); getFIFOBytes(fifoBuffer, fifoCount); DEBUG_PRINTLN(F("Reading interrupt status...")); mpuIntStatus = getIntStatus(); DEBUG_PRINT(F("Current interrupt status=")); DEBUG_PRINTLNF(mpuIntStatus, HEX); DEBUG_PRINTLN(F("Writing final memory update 7/7 (function unknown)...")); for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); DEBUG_PRINTLN(F("DMP is good to go! Finally.")); DEBUG_PRINTLN(F("Disabling DMP (you turn it on later)...")); setDMPEnabled(false); DEBUG_PRINTLN(F("Setting up internal 42-byte (default) DMP packet buffer...")); dmpPacketSize = 42; /*if ((dmpPacketBuffer = (uint8_t *)malloc(42)) == 0) { return 3; // TODO: proper error code for no memory }*/ DEBUG_PRINTLN(F("Resetting FIFO and clearing INT status one last time...")); resetFIFO(); getIntStatus(); } else { DEBUG_PRINTLN(F("ERROR! DMP configuration verification failed.")); return 2; // configuration block loading failed } } else { DEBUG_PRINTLN(F("ERROR! DMP code verification failed.")); return 1; // main binary block loading failed } return 0; // success } bool MPU6050::dmpPacketAvailable() { return getFIFOCount() >= dmpGetFIFOPacketSize(); } // uint8_t MPU6050::dmpSetFIFORate(uint8_t fifoRate); // uint8_t MPU6050::dmpGetFIFORate(); // uint8_t MPU6050::dmpGetSampleStepSizeMS(); // uint8_t MPU6050::dmpGetSampleFrequency(); // int32_t MPU6050::dmpDecodeTemperature(int8_t tempReg); //uint8_t MPU6050::dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority); //uint8_t MPU6050::dmpUnregisterFIFORateProcess(inv_obj_func func); //uint8_t MPU6050::dmpRunFIFORateProcesses(); // uint8_t MPU6050::dmpSendQuaternion(uint_fast16_t accuracy); // uint8_t MPU6050::dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy); // uint8_t MPU6050::dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy); // uint8_t MPU6050::dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy); // uint8_t MPU6050::dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy); // uint8_t MPU6050::dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy); // uint8_t MPU6050::dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy); // uint8_t MPU6050::dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy); // uint8_t MPU6050::dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy); // uint8_t MPU6050::dmpSendPacketNumber(uint_fast16_t accuracy); // uint8_t MPU6050::dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy); // uint8_t MPU6050::dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy); uint8_t MPU6050::dmpGetAccel(int32_t *data, const uint8_t* packet) { // TODO: accommodate different arrangements of sent data (ONLY default supported now) if (packet == 0) packet = dmpPacketBuffer; data[0] = ((packet[28] << 24) + (packet[29] << 16) + (packet[30] << 8) + packet[31]); data[1] = ((packet[32] << 24) + (packet[33] << 16) + (packet[34] << 8) + packet[35]); data[2] = ((packet[36] << 24) + (packet[37] << 16) + (packet[38] << 8) + packet[39]); return 0; } uint8_t MPU6050::dmpGetAccel(int16_t *data, const uint8_t* packet) { // TODO: accommodate different arrangements of sent data (ONLY default supported now) if (packet == 0) packet = dmpPacketBuffer; data[0] = (packet[28] << 8) + packet[29]; data[1] = (packet[32] << 8) + packet[33]; data[2] = (packet[36] << 8) + packet[37]; return 0; } uint8_t MPU6050::dmpGetAccel(VectorInt16 *v, const uint8_t* packet) { // TODO: accommodate different arrangements of sent data (ONLY default supported now) if (packet == 0) packet = dmpPacketBuffer; v -> x = (packet[28] << 8) + packet[29]; v -> y = (packet[32] << 8) + packet[33]; v -> z = (packet[36] << 8) + packet[37]; return 0; } uint8_t MPU6050::dmpGetQuaternion(int32_t *data, const uint8_t* packet) { // TODO: accommodate different arrangements of sent data (ONLY default supported now) if (packet == 0) packet = dmpPacketBuffer; data[0] = ((packet[0] << 24) + (packet[1] << 16) + (packet[2] << 8) + packet[3]); data[1] = ((packet[4] << 24) + (packet[5] << 16) + (packet[6] << 8) + packet[7]); data[2] = ((packet[8] << 24) + (packet[9] << 16) + (packet[10] << 8) + packet[11]); data[3] = ((packet[12] << 24) + (packet[13] << 16) + (packet[14] << 8) + packet[15]); return 0; } uint8_t MPU6050::dmpGetQuaternion(int16_t *data, const uint8_t* packet) { // TODO: accommodate different arrangements of sent data (ONLY default supported now) if (packet == 0) packet = dmpPacketBuffer; data[0] = ((packet[0] << 8) + packet[1]); data[1] = ((packet[4] << 8) + packet[5]); data[2] = ((packet[8] << 8) + packet[9]); data[3] = ((packet[12] << 8) + packet[13]); return 0; } uint8_t MPU6050::dmpGetQuaternion(Quaternion *q, const uint8_t* packet) { // TODO: accommodate different arrangements of sent data (ONLY default supported now) int16_t qI[4]; uint8_t status = dmpGetQuaternion(qI, packet); if (status == 0) { q -> w = (float)qI[0] / 16384.0f; q -> x = (float)qI[1] / 16384.0f; q -> y = (float)qI[2] / 16384.0f; q -> z = (float)qI[3] / 16384.0f; return 0; } return status; // int16 return value, indicates error if this line is reached } // uint8_t MPU6050::dmpGet6AxisQuaternion(long *data, const uint8_t* packet); // uint8_t MPU6050::dmpGetRelativeQuaternion(long *data, const uint8_t* packet); uint8_t MPU6050::dmpGetGyro(int32_t *data, const uint8_t* packet) { // TODO: accommodate different arrangements of sent data (ONLY default supported now) if (packet == 0) packet = dmpPacketBuffer; data[0] = ((packet[16] << 24) + (packet[17] << 16) + (packet[18] << 8) + packet[19]); data[1] = ((packet[20] << 24) + (packet[21] << 16) + (packet[22] << 8) + packet[23]); data[2] = ((packet[24] << 24) + (packet[25] << 16) + (packet[26] << 8) + packet[27]); return 0; } uint8_t MPU6050::dmpGetGyro(int16_t *data, const uint8_t* packet) { // TODO: accommodate different arrangements of sent data (ONLY default supported now) if (packet == 0) packet = dmpPacketBuffer; data[0] = (packet[16] << 8) + packet[17]; data[1] = (packet[20] << 8) + packet[21]; data[2] = (packet[24] << 8) + packet[25]; return 0; } uint8_t MPU6050::dmpGetGyro(VectorInt16 *v, const uint8_t* packet) { // TODO: accommodate different arrangements of sent data (ONLY default supported now) if (packet == 0) packet = dmpPacketBuffer; v -> x = (packet[16] << 8) + packet[17]; v -> y = (packet[20] << 8) + packet[21]; v -> z = (packet[24] << 8) + packet[25]; return 0; } // uint8_t MPU6050::dmpSetLinearAccelFilterCoefficient(float coef); // uint8_t MPU6050::dmpGetLinearAccel(long *data, const uint8_t* packet); uint8_t MPU6050::dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity) { // get rid of the gravity component (+1g = +8192 in standard DMP FIFO packet, sensitivity is 2g) v -> x = vRaw -> x - gravity -> x*8192; v -> y = vRaw -> y - gravity -> y*8192; v -> z = vRaw -> z - gravity -> z*8192; return 0; } // uint8_t MPU6050::dmpGetLinearAccelInWorld(long *data, const uint8_t* packet); uint8_t MPU6050::dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q) { // rotate measured 3D acceleration vector into original state // frame of reference based on orientation quaternion memcpy(v, vReal, sizeof(VectorInt16)); v -> rotate(q); return 0; } // uint8_t MPU6050::dmpGetGyroAndAccelSensor(long *data, const uint8_t* packet); // uint8_t MPU6050::dmpGetGyroSensor(long *data, const uint8_t* packet); // uint8_t MPU6050::dmpGetControlData(long *data, const uint8_t* packet); // uint8_t MPU6050::dmpGetTemperature(long *data, const uint8_t* packet); // uint8_t MPU6050::dmpGetGravity(long *data, const uint8_t* packet); uint8_t MPU6050::dmpGetGravity(VectorFloat *v, Quaternion *q) { v -> x = 2 * (q -> x*q -> z - q -> w*q -> y); v -> y = 2 * (q -> w*q -> x + q -> y*q -> z); v -> z = q -> w*q -> w - q -> x*q -> x - q -> y*q -> y + q -> z*q -> z; return 0; } // uint8_t MPU6050::dmpGetUnquantizedAccel(long *data, const uint8_t* packet); // uint8_t MPU6050::dmpGetQuantizedAccel(long *data, const uint8_t* packet); // uint8_t MPU6050::dmpGetExternalSensorData(long *data, int size, const uint8_t* packet); // uint8_t MPU6050::dmpGetEIS(long *data, const uint8_t* packet); uint8_t MPU6050::dmpGetEuler(float *data, Quaternion *q) { data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1); // psi data[1] = -asin(2*q -> x*q -> z + 2*q -> w*q -> y); // theta data[2] = atan2(2*q -> y*q -> z - 2*q -> w*q -> x, 2*q -> w*q -> w + 2*q -> z*q -> z - 1); // phi return 0; } uint8_t MPU6050::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) { // yaw: (about Z axis) data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1); // pitch: (nose up/down, about Y axis) data[1] = atan(gravity -> x / sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z)); // roll: (tilt left/right, about X axis) data[2] = atan(gravity -> y / sqrt(gravity -> x*gravity -> x + gravity -> z*gravity -> z)); return 0; } // uint8_t MPU6050::dmpGetAccelFloat(float *data, const uint8_t* packet); // uint8_t MPU6050::dmpGetQuaternionFloat(float *data, const uint8_t* packet); uint8_t MPU6050::dmpProcessFIFOPacket(const unsigned char *dmpData) { /*for (uint8_t k = 0; k < dmpPacketSize; k++) { if (dmpData[k] < 0x10) Serial.print("0"); Serial.print(dmpData[k], HEX); Serial.print(" "); } Serial.print("\n");*/ //Serial.println((uint16_t)dmpPacketBuffer); return 0; } uint8_t MPU6050::dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed) { uint8_t status; uint8_t buf[dmpPacketSize]; for (uint8_t i = 0; i < numPackets; i++) { // read packet from FIFO getFIFOBytes(buf, dmpPacketSize); // process packet if ((status = dmpProcessFIFOPacket(buf)) > 0) return status; // increment external process count variable, if supplied if (processed != 0) *processed++; } return 0; } // uint8_t MPU6050::dmpSetFIFOProcessedCallback(void (*func) (void)); // uint8_t MPU6050::dmpInitFIFOParam(); // uint8_t MPU6050::dmpCloseFIFO(); // uint8_t MPU6050::dmpSetGyroDataSource(uint_fast8_t source); // uint8_t MPU6050::dmpDecodeQuantizedAccel(); // uint32_t MPU6050::dmpGetGyroSumOfSquare(); // uint32_t MPU6050::dmpGetAccelSumOfSquare(); // void MPU6050::dmpOverrideQuaternion(long *q); uint16_t MPU6050::dmpGetFIFOPacketSize() { return dmpPacketSize; } #endif /* _MPU6050_6AXIS_MOTIONAPPS20_H_ */

I am using MPU6050 along with ATtiny2313 avr microcontroller. I want to read the values generated by the DMP in the MPU6050 IMU, but i am not sure as to what to do. i.e. which register contains what value, which register to read ... I tried to understand things by example your code but could not understand anything. Could someone please help me in some way, maybe by pointing me in some direction or some resources. I am trying to write my one code in Embedded C for interfacing and i am succesful in reading in accelerometer and gyroscope values from the MPU6050 IC.