Search the Community

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

#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

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!

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

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.

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.

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.

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

Hi, I know that there are some eamples showing how to put magnetometer data into DMP. For example this one: http://www.i2cdevlib.com/forums/topic/111-arduino-example-sketch-to-read-magnetometer-while-dmp-is-on/ but it does not make any sensor fusion. I couldn't find any library which would make magnetometer fusion inside DMP. Does such library exist? Maybe code in this file MPU6050_9Axis_MotionApps41.h make DMP+magnetometer fusion? I couldn't find any tips in the code and right now I am not able to test it. So the question is: does such library exist or the only way to do this is to make own fiters (for example complementary filter)? Maybe there is some MPU9150 library that can be used with MPU6050+magnetometer?

Hello Forum User, First off, thank you for taking the time to read this. Second, I require some assistance in the rewriting of the InvenSence DMP lib for the TI microcontroller for use with the Arduino Uno and MPU6050. I have managed to remove most of the syntax errors that occured when switching from .c to .cpp, and removed the TI microcontroller specific code, but I am having problems with utilizing the Wire.h twi libraries for the I2C communication (specificly writing a function to call the twi libs when the i2c_write or i2c_read functions are called.) Ideally, it would use the included Wire.h library or the i2cdev library for the i2c communication. The main reason for this rewrite is to provide the ability of the users to call all of the listed functions of the DMP. In my case, this is primarally for the Pedometer functions which are not included in other libraries, but it can be used for the other functions as well. Below is a link to the dropbox file with the original library and the modified library. https://www.dropbox.com/s/4umn96yhh5hb9gu/eMPL_Rewrite.zip?dl=0 Thank you very much for your assistance in the matter. -Amatoxin-

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

I would like to say thank Jeff for Greate library! Does somebody know how to switch off autocaliration? The one thats take place evry time DMP feels it is stationary? (I looked through the function declaration in lib and failed to find. I also looked throgh specs and was also unable to find).

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

The demo DMP sketch continually locks up after anywhere between 3-20 seconds into running. The demo code is unmodified in any way other than commenting out a few of the #define's to limit the output. I get no errors and no FIFO overflows, results appear on the serial monitor...but then the program just hangs. I have a clean setup here...nothing else attached to the Arduino. Suggestions??