├── .gitignore ├── references.md ├── dev ├── I2Cdev │ └── I2Cdev │ │ ├── library.json │ │ ├── keywords.txt │ │ └── I2Cdev.h └── MPU6050 │ ├── Examples │ ├── MPU6050_raw │ │ └── MPU6050_raw.ino │ └── MPU6050_DMP6 │ │ ├── Processing │ │ └── MPUTeapot │ │ │ └── MPUTeapot.pde │ │ └── MPU6050_DMP6.ino │ ├── helper_3dmath.h │ ├── MPU6050_6Axis_MotionApps20.h │ ├── MPU6050.h │ └── MPU6050_9Axis_MotionApps41.h ├── proof_of_concepts ├── emg_poc │ └── emg_poc.ino ├── flexsensor_poc │ └── flexsensor_poc.ino └── gyroscope_poc │ └── gyroscope_poc.ino ├── README.md └── final ├── emg_SP └── emg_SP.ino └── gyro_3719 └── gyro_3719.ino /.gitignore: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /references.md: -------------------------------------------------------------------------------- 1 | references for ece196Final: 2 | 3 | https://maker.pro/arduino/tutorial/how-to-interface-arduino-and-the-mpu-6050-sensor 4 | 5 | https://learn.sparkfun.com/tutorials/flex-sensor-hookup-guide/all 6 | 7 | 8 | https://photos.app.goo.gl/vSJ5FrvoZYVn1kdG8 9 | -------------------------------------------------------------------------------- /dev/I2Cdev/I2Cdev/library.json: -------------------------------------------------------------------------------- 1 | { 2 | "name": "I2Cdevlib-Core", 3 | "keywords": "i2cdevlib, i2c", 4 | "description": "The I2C Device Library (I2Cdevlib) is a collection of uniform and well-documented classes to provide simple and intuitive interfaces to I2C devices.", 5 | "include": "Arduino/I2Cdev", 6 | "repository": 7 | { 8 | "type": "git", 9 | "url": "https://github.com/jrowberg/i2cdevlib.git" 10 | }, 11 | "frameworks": "arduino", 12 | "platforms": "atmelavr" 13 | } 14 | -------------------------------------------------------------------------------- /proof_of_concepts/emg_poc/emg_poc.ino: -------------------------------------------------------------------------------- 1 | /* 2 | This snippet is to see how the EMG works with the Arduino. 3 | 4 | The ouput is connected to A3, and at first when we DIDNT connect the ardunio ground 5 | to the circuit ground, it acted like so: 6 | 7 | 1. Reading was aroudn 1023, arm unlfex 8 | 2. Arm flex, reading eventually go to 0 9 | 3. Arm flex again, readings dont change, then go back to 1020 after a few seconds 10 | 11 | Whwen we DID connect the grounds together, the readings were all over the place (100-400 12 | when not flexing). Soooooo 🤷 13 | */ 14 | 15 | const int EMG_PIN = A3; // Pin connected to voltage divider output 16 | 17 | void setup() 18 | { 19 | Serial.begin(9600); 20 | pinMode(EMG_PIN, INPUT); 21 | } 22 | 23 | void loop() 24 | { 25 | int emgRead = analogRead(EMG_PIN); 26 | Serial.println("emg: " + String(emgRead)); 27 | delay(250); 28 | } 29 | -------------------------------------------------------------------------------- /dev/I2Cdev/I2Cdev/keywords.txt: -------------------------------------------------------------------------------- 1 | ####################################### 2 | # Syntax Coloring Map For I2Cdev 3 | ####################################### 4 | 5 | ####################################### 6 | # Datatypes (KEYWORD1) 7 | ####################################### 8 | I2Cdev KEYWORD1 9 | 10 | ####################################### 11 | # Methods and Functions (KEYWORD2) 12 | ####################################### 13 | 14 | readBit KEYWORD2 15 | readBitW KEYWORD2 16 | readBits KEYWORD2 17 | readBitsW KEYWORD2 18 | readByte KEYWORD2 19 | readBytes KEYWORD2 20 | readWord KEYWORD2 21 | readWords KEYWORD2 22 | writeBit KEYWORD2 23 | writeBitW KEYWORD2 24 | writeBits KEYWORD2 25 | writeBitsW KEYWORD2 26 | writeByte KEYWORD2 27 | writeBytes KEYWORD2 28 | writeWord KEYWORD2 29 | writeWords KEYWORD2 30 | 31 | ####################################### 32 | # Instances (KEYWORD2) 33 | ####################################### 34 | 35 | ####################################### 36 | # Constants (LITERAL1) 37 | ####################################### 38 | 39 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # ECE 196 Final Project - N.S.A. 2 | 3 | A glove that controls a robotic arm. 4 | 5 | This repository contains code that was used in creating our final project for eCE 196. 6 | 7 | Our project was creating a glove that contained sensors that controlled the PiB Robitic Arm. Most of the code hosted here is for the sensors that interacted with the Arduino (EMG, gyroscope/acceleromter, flex sesnors, etc.). 8 | 9 | Our full documentation for this project can be find in our final report. 10 | 11 | There's also miscellaneous files for other things (EAGLE, etc.). 12 | 13 | ## Contents 14 | 15 | ### `/proof_of_concepts` 16 | 17 | This folder contains a directory for each sensor, that contains a small Arduino script that acts as a "proof of concept". It's meant for students who have never worked with the sensors in question, and just want to get a simple "hello world" set up to make sure their sensor+code is working properly. 18 | 19 | ### `/final` 20 | 21 | The final code that we used to get the glove to work, as a MVP. 22 | 23 | ### `/dev` 24 | 25 | Some depedencies you'll need for the gyroscope/accelerometer libraries. 26 | -------------------------------------------------------------------------------- /final/emg_SP/emg_SP.ino: -------------------------------------------------------------------------------- 1 | #include 2 | #define baseServoPin 3 3 | 4 | Servo baseServo; 5 | 6 | const int numReadings=15; //size of readings array 7 | int readings[numReadings]; //readings from the analog input 8 | int readIndex=0; //index of current reading 9 | int total=0; //running total 10 | int average=0; //average 11 | 12 | int EMG=A3; //signal 13 | int mapping; 14 | int counter = 0; 15 | int isOn = 0; 16 | 17 | 18 | void setup() { 19 | // put your setup code here, to run once: 20 | baseServo.attach(baseServoPin); 21 | baseServo.write(150); 22 | Serial.begin(9600); 23 | //initialize: 24 | for (int thisReading=0; thisReading < numReadings; thisReading++) { 25 | readings[thisReading]=0; 26 | } 27 | } 28 | 29 | void loop() { 30 | 31 | // put your main code here, to run repeatedly: 32 | total=total-readings[readIndex]; //subtract last reading 33 | readings[readIndex]=analogRead(EMG); //read EMG 34 | total=total+readings[readIndex]; //add that reading to total 35 | readIndex=readIndex+1; //advance to next position 36 | 37 | //for end of array: 38 | if (readIndex >= numReadings) { 39 | readIndex=0; 40 | } 41 | average=total/numReadings; //calculate average 42 | Serial.println(average); 43 | 44 | //mapping = map(average, 240, 0, 180, 180); 45 | 46 | if (average >= 230){ 47 | isOn = 1; 48 | } 49 | if (isOn == 1){ 50 | baseServo.write(100); 51 | isOn++; 52 | } 53 | if (isOn == 2){ 54 | baseServo.write(145); 55 | isOn = 0; 56 | } 57 | 58 | delay(1); //stability 59 | //baseServo.write(average); 60 | } 61 | -------------------------------------------------------------------------------- /proof_of_concepts/flexsensor_poc/flexsensor_poc.ino: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | Flex_Sensor_Example.ino 3 | Example sketch for SparkFun's flex sensors 4 | (https://www.sparkfun.com/products/10264) 5 | Jim Lindblom @ SparkFun Electronics 6 | April 28, 2016 7 | 8 | Create a voltage divider circuit combining a flex sensor with a 47k resistor. 9 | - The resistor should connect from A0 to GND. 10 | - The flex sensor should connect from A0 to 3.3V 11 | As the resistance of the flex sensor increases (meaning it's being bent), the 12 | voltage at A0 should decrease. 13 | 14 | Development environment specifics: 15 | Arduino 1.6.7 16 | ******************************************************************************/ 17 | //for the robotic arm implementation 18 | #include 19 | #define elbowServoPin 6 20 | #define shoulderServoPin 5 21 | 22 | Servo elbowServo; 23 | Servo shoulderServo; 24 | 25 | const int FLEX_PIN = A0; // Pin connected to voltage divider output 26 | const int FLEX_PIN2 = A1; 27 | 28 | // Measure the voltage at 5V and the actual resistance of your 29 | // 47k resistor, and enter them below: 30 | const float VCC = 4.98; // Measured voltage of Ardunio 5V line 31 | const float R_DIV = 330.0; // Measured resistance of 3.3k resistor 32 | 33 | // Upload the code, then try to adjust these values to more 34 | // accurately calculate bend degree. 35 | const float STRAIGHT_RESISTANCE = 12000.0; // resistance when straight 36 | const float BEND_RESISTANCE = 50000.0; // resistance at 90 deg 37 | 38 | void setup() 39 | { 40 | elbowServo.attach(elbowServoPin); 41 | shoulderServo.attach(shoulderServoPin); 42 | Serial.begin(9600); 43 | pinMode(FLEX_PIN, INPUT); 44 | pinMode(FLEX_PIN2, INPUT); 45 | } 46 | 47 | void loop() 48 | { 49 | // Read the ADC, and calculate voltage and resistance from it 50 | int flexADC = analogRead(FLEX_PIN); 51 | int flexADC2 = analogRead(FLEX_PIN2); 52 | float flexV = flexADC * VCC / 1023.0; 53 | float flexV2 = flexADC2 * VCC / 1023.0; 54 | float flexR = R_DIV * (VCC / flexV - 1.0); 55 | float flexR2 = R_DIV * (VCC / flexV2 - 1.0); 56 | 57 | Serial.println("Resistance: " + String(flexR) + " ohms"); 58 | Serial.println("Resistance: " + String(flexR2) + " ohms"); 59 | 60 | // Use the calculated resistance to estimate the sensor's 61 | // bend angle: 62 | 63 | float angle = map(flexR, STRAIGHT_RESISTANCE, BEND_RESISTANCE, 64 | 50, 100.0); 65 | 66 | float angle2 = map(flexR2, STRAIGHT_RESISTANCE, BEND_RESISTANCE, 67 | 50, 100.0); 68 | 69 | Serial.println("Bend: " + String(angle) + " degrees"); 70 | Serial.println(); 71 | Serial.println("Bend: " + String(angle2) + " degrees"); 72 | Serial.println(); 73 | elbowServo.write(angle); 74 | shoulderServo.write(angle2); 75 | 76 | //delay(500); 77 | } 78 | -------------------------------------------------------------------------------- /proof_of_concepts/gyroscope_poc/gyroscope_poc.ino: -------------------------------------------------------------------------------- 1 | // https://create.arduino.cc/editor/asg017/12c8ef32-3df8-41e1-9892-e65618494fe6/preview 2 | #include 3 | const int MPU_addr=0x68; // I2C address of the MPU-6050 4 | int16_t AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ; 5 | 6 | int16_t max_AcX,max_AcY,max_AcZ,max_Tmp,max_GyX,max_GyY,max_GyZ; 7 | int16_t min_AcX,min_AcY,min_AcZ,min_Tmp,min_GyX,min_GyY,min_GyZ; 8 | 9 | 10 | void setup(){ 11 | max_AcX = -200000; 12 | max_AcY= -200000; 13 | max_AcZ= -200000; 14 | max_Tmp = -200000; 15 | max_GyX = -200000; 16 | max_GyY= -200000; 17 | max_GyZ= -200000; 18 | 19 | min_AcX = 200000; 20 | min_AcY= 200000; 21 | min_AcZ= 200000; 22 | min_Tmp =200000; 23 | min_GyX = 200000; 24 | min_GyY= 200000; 25 | min_GyZ= 200000; 26 | Wire.begin(); 27 | Wire.beginTransmission(MPU_addr); 28 | Wire.write(0x6B); // PWR_MGMT_1 register 29 | Wire.write(0); // set to zero (wakes up the MPU-6050) 30 | Wire.endTransmission(true); 31 | Serial.begin(9600); 32 | } 33 | void loop(){ 34 | Wire.beginTransmission(MPU_addr); 35 | Wire.write(0x3B); // starting with register 0x3B (ACCEL_XOUT_H) 36 | Wire.endTransmission(false); 37 | Wire.requestFrom(MPU_addr,14,true); // request a total of 14 registers 38 | AcX=Wire.read()<<8|Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L) 39 | AcY=Wire.read()<<8|Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L) 40 | AcZ=Wire.read()<<8|Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L) 41 | Tmp=Wire.read()<<8|Wire.read(); // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L) 42 | GyX=Wire.read()<<8|Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L) 43 | GyY=Wire.read()<<8|Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L) 44 | GyZ=Wire.read()<<8|Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L) 45 | /* 46 | Serial.print("AcX = "); Serial.print(AcX); 47 | Serial.print(" | AcY = "); Serial.print(AcY); 48 | Serial.print(" | AcZ = "); Serial.print(AcZ); 49 | Serial.print(" | Tmp = "); Serial.print(Tmp/340.00+36.53); //equation for temperature in degrees C from datasheet 50 | Serial.print(" | GyX = "); Serial.print(GyX); 51 | Serial.print(" | GyY = "); Serial.print(GyY); 52 | Serial.print(" | GyZ = "); Serial.println(GyZ); 53 | //*/ 54 | if(AcX > max_AcX) 55 | max_AcX = AcX; 56 | if(AcY > max_AcY) 57 | max_AcY = AcY; 58 | if(AcZ > max_AcZ) 59 | max_AcZ = AcZ; 60 | if(Tmp > max_Tmp) 61 | max_Tmp = Tmp; 62 | if(GyX > max_GyX) 63 | max_GyX = GyX; 64 | if(GyY > max_GyY) 65 | max_GyY = GyY; 66 | if(GyZ > max_GyZ) 67 | max_GyZ = GyZ; 68 | 69 | if(AcX < min_AcX) 70 | min_AcX = AcX; 71 | if(AcY < min_AcY) 72 | min_AcY = AcY; 73 | if(AcZ < max_AcZ) 74 | min_AcZ = AcZ; 75 | if(Tmp < max_Tmp) 76 | min_Tmp = Tmp; 77 | if(GyX < max_GyX) 78 | min_GyX = GyX; 79 | if(GyY < max_GyY) 80 | min_GyY = GyY; 81 | if(GyZ < max_GyZ) 82 | min_GyZ = GyZ; 83 | 84 | Serial.print("AcX = "); Serial.print(min_AcX);Serial.print("_");Serial.print(max_AcX); 85 | Serial.print(" | AcY = "); Serial.print(min_AcY);Serial.print("_");Serial.print(max_AcY); 86 | Serial.print(" | AcZ = "); Serial.print(min_AcZ);Serial.print("_");Serial.print(max_AcZ); 87 | //Serial.print(" | Tmp = "); Serial.print(Tmp/340.00+36.53); //equation for temperature in degrees C from datasheet 88 | Serial.print(" | GyX = "); Serial.print(min_GyX);Serial.print("_");Serial.print(max_GyX); 89 | Serial.print(" | GyY = "); Serial.print(min_GyY);Serial.print("_");Serial.print(max_GyY); 90 | Serial.print(" | GyZ = "); Serial.println(min_GyZ);Serial.print("_");Serial.print(max_GyZ); 91 | 92 | delay(333); 93 | } 94 | -------------------------------------------------------------------------------- /dev/MPU6050/Examples/MPU6050_raw/MPU6050_raw.ino: -------------------------------------------------------------------------------- 1 | // I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class 2 | // 10/7/2011 by Jeff Rowberg 3 | // Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib 4 | // 5 | // Changelog: 6 | // 2013-05-08 - added multiple output formats 7 | // - added seamless Fastwire support 8 | // 2011-10-07 - initial release 9 | 10 | /* ============================================ 11 | I2Cdev device library code is placed under the MIT license 12 | Copyright (c) 2011 Jeff Rowberg 13 | 14 | Permission is hereby granted, free of charge, to any person obtaining a copy 15 | of this software and associated documentation files (the "Software"), to deal 16 | in the Software without restriction, including without limitation the rights 17 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 18 | copies of the Software, and to permit persons to whom the Software is 19 | furnished to do so, subject to the following conditions: 20 | 21 | The above copyright notice and this permission notice shall be included in 22 | all copies or substantial portions of the Software. 23 | 24 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 25 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 26 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 27 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 28 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 29 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 30 | THE SOFTWARE. 31 | =============================================== 32 | */ 33 | 34 | // I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files 35 | // for both classes must be in the include path of your project 36 | #include "I2Cdev.h" 37 | #include "MPU6050.h" 38 | 39 | // Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation 40 | // is used in I2Cdev.h 41 | #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE 42 | #include "Wire.h" 43 | #endif 44 | 45 | // class default I2C address is 0x68 46 | // specific I2C addresses may be passed as a parameter here 47 | // AD0 low = 0x68 (default for InvenSense evaluation board) 48 | // AD0 high = 0x69 49 | MPU6050 accelgyro; 50 | //MPU6050 accelgyro(0x69); // <-- use for AD0 high 51 | 52 | int16_t ax, ay, az; 53 | int16_t gx, gy, gz; 54 | 55 | 56 | 57 | // uncomment "OUTPUT_READABLE_ACCELGYRO" if you want to see a tab-separated 58 | // list of the accel X/Y/Z and then gyro X/Y/Z values in decimal. Easy to read, 59 | // not so easy to parse, and slow(er) over UART. 60 | #define OUTPUT_READABLE_ACCELGYRO 61 | 62 | // uncomment "OUTPUT_BINARY_ACCELGYRO" to send all 6 axes of data as 16-bit 63 | // binary, one right after the other. This is very fast (as fast as possible 64 | // without compression or data loss), and easy to parse, but impossible to read 65 | // for a human. 66 | //#define OUTPUT_BINARY_ACCELGYRO 67 | 68 | 69 | #define LED_PIN 13 70 | bool blinkState = false; 71 | 72 | void setup() { 73 | // join I2C bus (I2Cdev library doesn't do this automatically) 74 | #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE 75 | Wire.begin(); 76 | #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE 77 | Fastwire::setup(400, true); 78 | #endif 79 | 80 | // initialize serial communication 81 | // (38400 chosen because it works as well at 8MHz as it does at 16MHz, but 82 | // it's really up to you depending on your project) 83 | Serial.begin(38400); 84 | 85 | // initialize device 86 | Serial.println("Initializing I2C devices..."); 87 | accelgyro.initialize(); 88 | 89 | // verify connection 90 | Serial.println("Testing device connections..."); 91 | Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed"); 92 | 93 | // use the code below to change accel/gyro offset values 94 | /* 95 | Serial.println("Updating internal sensor offsets..."); 96 | // -76 -2359 1688 0 0 0 97 | Serial.print(accelgyro.getXAccelOffset()); Serial.print("\t"); // -76 98 | Serial.print(accelgyro.getYAccelOffset()); Serial.print("\t"); // -2359 99 | Serial.print(accelgyro.getZAccelOffset()); Serial.print("\t"); // 1688 100 | Serial.print(accelgyro.getXGyroOffset()); Serial.print("\t"); // 0 101 | Serial.print(accelgyro.getYGyroOffset()); Serial.print("\t"); // 0 102 | Serial.print(accelgyro.getZGyroOffset()); Serial.print("\t"); // 0 103 | Serial.print("\n"); 104 | accelgyro.setXGyroOffset(220); 105 | accelgyro.setYGyroOffset(76); 106 | accelgyro.setZGyroOffset(-85); 107 | Serial.print(accelgyro.getXAccelOffset()); Serial.print("\t"); // -76 108 | Serial.print(accelgyro.getYAccelOffset()); Serial.print("\t"); // -2359 109 | Serial.print(accelgyro.getZAccelOffset()); Serial.print("\t"); // 1688 110 | Serial.print(accelgyro.getXGyroOffset()); Serial.print("\t"); // 0 111 | Serial.print(accelgyro.getYGyroOffset()); Serial.print("\t"); // 0 112 | Serial.print(accelgyro.getZGyroOffset()); Serial.print("\t"); // 0 113 | Serial.print("\n"); 114 | */ 115 | 116 | // configure Arduino LED for 117 | pinMode(LED_PIN, OUTPUT); 118 | } 119 | 120 | void loop() { 121 | // read raw accel/gyro measurements from device 122 | accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); 123 | 124 | // these methods (and a few others) are also available 125 | //accelgyro.getAcceleration(&ax, &ay, &az); 126 | //accelgyro.getRotation(&gx, &gy, &gz); 127 | 128 | #ifdef OUTPUT_READABLE_ACCELGYRO 129 | // display tab-separated accel/gyro x/y/z values 130 | Serial.print("a/g:\t"); 131 | Serial.print(ax); Serial.print("\t"); 132 | Serial.print(ay); Serial.print("\t"); 133 | Serial.print(az); Serial.print("\t"); 134 | Serial.print(gx); Serial.print("\t"); 135 | Serial.print(gy); Serial.print("\t"); 136 | Serial.println(gz); 137 | #endif 138 | 139 | #ifdef OUTPUT_BINARY_ACCELGYRO 140 | Serial.write((uint8_t)(ax >> 8)); Serial.write((uint8_t)(ax & 0xFF)); 141 | Serial.write((uint8_t)(ay >> 8)); Serial.write((uint8_t)(ay & 0xFF)); 142 | Serial.write((uint8_t)(az >> 8)); Serial.write((uint8_t)(az & 0xFF)); 143 | Serial.write((uint8_t)(gx >> 8)); Serial.write((uint8_t)(gx & 0xFF)); 144 | Serial.write((uint8_t)(gy >> 8)); Serial.write((uint8_t)(gy & 0xFF)); 145 | Serial.write((uint8_t)(gz >> 8)); Serial.write((uint8_t)(gz & 0xFF)); 146 | #endif 147 | 148 | // blink LED to indicate activity 149 | blinkState = !blinkState; 150 | digitalWrite(LED_PIN, blinkState); 151 | } 152 | -------------------------------------------------------------------------------- /dev/MPU6050/helper_3dmath.h: -------------------------------------------------------------------------------- 1 | // I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class, 3D math helper 2 | // 6/5/2012 by Jeff Rowberg 3 | // Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib 4 | // 5 | // Changelog: 6 | // 2012-06-05 - add 3D math helper file to DMP6 example sketch 7 | 8 | /* ============================================ 9 | I2Cdev device library code is placed under the MIT license 10 | Copyright (c) 2012 Jeff Rowberg 11 | 12 | Permission is hereby granted, free of charge, to any person obtaining a copy 13 | of this software and associated documentation files (the "Software"), to deal 14 | in the Software without restriction, including without limitation the rights 15 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 16 | copies of the Software, and to permit persons to whom the Software is 17 | furnished to do so, subject to the following conditions: 18 | 19 | The above copyright notice and this permission notice shall be included in 20 | all copies or substantial portions of the Software. 21 | 22 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 23 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 24 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 25 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 26 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 27 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 28 | THE SOFTWARE. 29 | =============================================== 30 | */ 31 | 32 | #ifndef _HELPER_3DMATH_H_ 33 | #define _HELPER_3DMATH_H_ 34 | 35 | class Quaternion { 36 | public: 37 | float w; 38 | float x; 39 | float y; 40 | float z; 41 | 42 | Quaternion() { 43 | w = 1.0f; 44 | x = 0.0f; 45 | y = 0.0f; 46 | z = 0.0f; 47 | } 48 | 49 | Quaternion(float nw, float nx, float ny, float nz) { 50 | w = nw; 51 | x = nx; 52 | y = ny; 53 | z = nz; 54 | } 55 | 56 | Quaternion getProduct(Quaternion q) { 57 | // Quaternion multiplication is defined by: 58 | // (Q1 * Q2).w = (w1w2 - x1x2 - y1y2 - z1z2) 59 | // (Q1 * Q2).x = (w1x2 + x1w2 + y1z2 - z1y2) 60 | // (Q1 * Q2).y = (w1y2 - x1z2 + y1w2 + z1x2) 61 | // (Q1 * Q2).z = (w1z2 + x1y2 - y1x2 + z1w2 62 | return Quaternion( 63 | w*q.w - x*q.x - y*q.y - z*q.z, // new w 64 | w*q.x + x*q.w + y*q.z - z*q.y, // new x 65 | w*q.y - x*q.z + y*q.w + z*q.x, // new y 66 | w*q.z + x*q.y - y*q.x + z*q.w); // new z 67 | } 68 | 69 | Quaternion getConjugate() { 70 | return Quaternion(w, -x, -y, -z); 71 | } 72 | 73 | float getMagnitude() { 74 | return sqrt(w*w + x*x + y*y + z*z); 75 | } 76 | 77 | void normalize() { 78 | float m = getMagnitude(); 79 | w /= m; 80 | x /= m; 81 | y /= m; 82 | z /= m; 83 | } 84 | 85 | Quaternion getNormalized() { 86 | Quaternion r(w, x, y, z); 87 | r.normalize(); 88 | return r; 89 | } 90 | }; 91 | 92 | class VectorInt16 { 93 | public: 94 | int16_t x; 95 | int16_t y; 96 | int16_t z; 97 | 98 | VectorInt16() { 99 | x = 0; 100 | y = 0; 101 | z = 0; 102 | } 103 | 104 | VectorInt16(int16_t nx, int16_t ny, int16_t nz) { 105 | x = nx; 106 | y = ny; 107 | z = nz; 108 | } 109 | 110 | float getMagnitude() { 111 | return sqrt(x*x + y*y + z*z); 112 | } 113 | 114 | void normalize() { 115 | float m = getMagnitude(); 116 | x /= m; 117 | y /= m; 118 | z /= m; 119 | } 120 | 121 | VectorInt16 getNormalized() { 122 | VectorInt16 r(x, y, z); 123 | r.normalize(); 124 | return r; 125 | } 126 | 127 | void rotate(Quaternion *q) { 128 | // http://www.cprogramming.com/tutorial/3d/quaternions.html 129 | // http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/transforms/index.htm 130 | // http://content.gpwiki.org/index.php/OpenGL:Tutorials:Using_Quaternions_to_represent_rotation 131 | // ^ or: http://webcache.googleusercontent.com/search?q=cache:xgJAp3bDNhQJ:content.gpwiki.org/index.php/OpenGL:Tutorials:Using_Quaternions_to_represent_rotation&hl=en&gl=us&strip=1 132 | 133 | // P_out = q * P_in * conj(q) 134 | // - P_out is the output vector 135 | // - q is the orientation quaternion 136 | // - P_in is the input vector (a*aReal) 137 | // - conj(q) is the conjugate of the orientation quaternion (q=[w,x,y,z], q*=[w,-x,-y,-z]) 138 | Quaternion p(0, x, y, z); 139 | 140 | // quaternion multiplication: q * p, stored back in p 141 | p = q -> getProduct(p); 142 | 143 | // quaternion multiplication: p * conj(q), stored back in p 144 | p = p.getProduct(q -> getConjugate()); 145 | 146 | // p quaternion is now [0, x', y', z'] 147 | x = p.x; 148 | y = p.y; 149 | z = p.z; 150 | } 151 | 152 | VectorInt16 getRotated(Quaternion *q) { 153 | VectorInt16 r(x, y, z); 154 | r.rotate(q); 155 | return r; 156 | } 157 | }; 158 | 159 | class VectorFloat { 160 | public: 161 | float x; 162 | float y; 163 | float z; 164 | 165 | VectorFloat() { 166 | x = 0; 167 | y = 0; 168 | z = 0; 169 | } 170 | 171 | VectorFloat(float nx, float ny, float nz) { 172 | x = nx; 173 | y = ny; 174 | z = nz; 175 | } 176 | 177 | float getMagnitude() { 178 | return sqrt(x*x + y*y + z*z); 179 | } 180 | 181 | void normalize() { 182 | float m = getMagnitude(); 183 | x /= m; 184 | y /= m; 185 | z /= m; 186 | } 187 | 188 | VectorFloat getNormalized() { 189 | VectorFloat r(x, y, z); 190 | r.normalize(); 191 | return r; 192 | } 193 | 194 | void rotate(Quaternion *q) { 195 | Quaternion p(0, x, y, z); 196 | 197 | // quaternion multiplication: q * p, stored back in p 198 | p = q -> getProduct(p); 199 | 200 | // quaternion multiplication: p * conj(q), stored back in p 201 | p = p.getProduct(q -> getConjugate()); 202 | 203 | // p quaternion is now [0, x', y', z'] 204 | x = p.x; 205 | y = p.y; 206 | z = p.z; 207 | } 208 | 209 | VectorFloat getRotated(Quaternion *q) { 210 | VectorFloat r(x, y, z); 211 | r.rotate(q); 212 | return r; 213 | } 214 | }; 215 | 216 | #endif /* _HELPER_3DMATH_H_ */ -------------------------------------------------------------------------------- /dev/MPU6050/Examples/MPU6050_DMP6/Processing/MPUTeapot/MPUTeapot.pde: -------------------------------------------------------------------------------- 1 | // I2C device class (I2Cdev) demonstration Processing sketch for MPU6050 DMP output 2 | // 6/20/2012 by Jeff Rowberg 3 | // Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib 4 | // 5 | // Changelog: 6 | // 2012-06-20 - initial release 7 | 8 | /* ============================================ 9 | I2Cdev device library code is placed under the MIT license 10 | Copyright (c) 2012 Jeff Rowberg 11 | 12 | Permission is hereby granted, free of charge, to any person obtaining a copy 13 | of this software and associated documentation files (the "Software"), to deal 14 | in the Software without restriction, including without limitation the rights 15 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 16 | copies of the Software, and to permit persons to whom the Software is 17 | furnished to do so, subject to the following conditions: 18 | 19 | The above copyright notice and this permission notice shall be included in 20 | all copies or substantial portions of the Software. 21 | 22 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 23 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 24 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 25 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 26 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 27 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 28 | THE SOFTWARE. 29 | =============================================== 30 | */ 31 | 32 | import processing.serial.*; 33 | import processing.opengl.*; 34 | import toxi.geom.*; 35 | import toxi.processing.*; 36 | 37 | // NOTE: requires ToxicLibs to be installed in order to run properly. 38 | // 1. Download from http://toxiclibs.org/downloads 39 | // 2. Extract into [userdir]/Processing/libraries 40 | // (location may be different on Mac/Linux) 41 | // 3. Run and bask in awesomeness 42 | 43 | ToxiclibsSupport gfx; 44 | 45 | Serial port; // The serial port 46 | char[] teapotPacket = new char[14]; // InvenSense Teapot packet 47 | int serialCount = 0; // current packet byte position 48 | int aligned = 0; 49 | int interval = 0; 50 | 51 | float[] q = new float[4]; 52 | Quaternion quat = new Quaternion(1, 0, 0, 0); 53 | 54 | float[] gravity = new float[3]; 55 | float[] euler = new float[3]; 56 | float[] ypr = new float[3]; 57 | 58 | void setup() { 59 | // 300px square viewport using OpenGL rendering 60 | size(300, 300, OPENGL); 61 | gfx = new ToxiclibsSupport(this); 62 | 63 | // setup lights and antialiasing 64 | lights(); 65 | smooth(); 66 | 67 | // display serial port list for debugging/clarity 68 | println(Serial.list()); 69 | 70 | // get the first available port (use EITHER this OR the specific port code below) 71 | String portName = "/dev/ttyUSB1"; 72 | 73 | // get a specific serial port (use EITHER this OR the first-available code above) 74 | //String portName = "COM4"; 75 | 76 | // open the serial port 77 | port = new Serial(this, portName, 115200); 78 | 79 | // send single character to trigger DMP init/start 80 | // (expected by MPU6050_DMP6 example Arduino sketch) 81 | port.write('r'); 82 | } 83 | 84 | void draw() { 85 | if (millis() - interval > 1000) { 86 | // resend single character to trigger DMP init/start 87 | // in case the MPU is halted/reset while applet is running 88 | port.write('r'); 89 | interval = millis(); 90 | } 91 | 92 | // black background 93 | background(0); 94 | 95 | // translate everything to the middle of the viewport 96 | pushMatrix(); 97 | translate(width / 2, height / 2); 98 | 99 | // 3-step rotation from yaw/pitch/roll angles (gimbal lock!) 100 | // ...and other weirdness I haven't figured out yet 101 | //rotateY(-ypr[0]); 102 | //rotateZ(-ypr[1]); 103 | //rotateX(-ypr[2]); 104 | 105 | // toxiclibs direct angle/axis rotation from quaternion (NO gimbal lock!) 106 | // (axis order [1, 3, 2] and inversion [-1, +1, +1] is a consequence of 107 | // different coordinate system orientation assumptions between Processing 108 | // and InvenSense DMP) 109 | float[] axis = quat.toAxisAngle(); 110 | rotate(axis[0], -axis[1], axis[3], axis[2]); 111 | 112 | // draw main body in red 113 | fill(255, 0, 0, 200); 114 | box(10, 10, 200); 115 | 116 | // draw front-facing tip in blue 117 | fill(0, 0, 255, 200); 118 | pushMatrix(); 119 | translate(0, 0, -120); 120 | rotateX(PI/2); 121 | drawCylinder(0, 20, 20, 8); 122 | popMatrix(); 123 | 124 | // draw wings and tail fin in green 125 | fill(0, 255, 0, 200); 126 | beginShape(TRIANGLES); 127 | vertex(-100, 2, 30); vertex(0, 2, -80); vertex(100, 2, 30); // wing top layer 128 | vertex(-100, -2, 30); vertex(0, -2, -80); vertex(100, -2, 30); // wing bottom layer 129 | vertex(-2, 0, 98); vertex(-2, -30, 98); vertex(-2, 0, 70); // tail left layer 130 | vertex( 2, 0, 98); vertex( 2, -30, 98); vertex( 2, 0, 70); // tail right layer 131 | endShape(); 132 | beginShape(QUADS); 133 | vertex(-100, 2, 30); vertex(-100, -2, 30); vertex( 0, -2, -80); vertex( 0, 2, -80); 134 | vertex( 100, 2, 30); vertex( 100, -2, 30); vertex( 0, -2, -80); vertex( 0, 2, -80); 135 | vertex(-100, 2, 30); vertex(-100, -2, 30); vertex(100, -2, 30); vertex(100, 2, 30); 136 | vertex(-2, 0, 98); vertex(2, 0, 98); vertex(2, -30, 98); vertex(-2, -30, 98); 137 | vertex(-2, 0, 98); vertex(2, 0, 98); vertex(2, 0, 70); vertex(-2, 0, 70); 138 | vertex(-2, -30, 98); vertex(2, -30, 98); vertex(2, 0, 70); vertex(-2, 0, 70); 139 | endShape(); 140 | 141 | popMatrix(); 142 | } 143 | 144 | void serialEvent(Serial port) { 145 | interval = millis(); 146 | while (port.available() > 0) { 147 | int ch = port.read(); 148 | print((char)ch); 149 | if (ch == '$') {serialCount = 0;} // this will help with alignment 150 | if (aligned < 4) { 151 | // make sure we are properly aligned on a 14-byte packet 152 | if (serialCount == 0) { 153 | if (ch == '$') aligned++; else aligned = 0; 154 | } else if (serialCount == 1) { 155 | if (ch == 2) aligned++; else aligned = 0; 156 | } else if (serialCount == 12) { 157 | if (ch == '\r') aligned++; else aligned = 0; 158 | } else if (serialCount == 13) { 159 | if (ch == '\n') aligned++; else aligned = 0; 160 | } 161 | //println(ch + " " + aligned + " " + serialCount); 162 | serialCount++; 163 | if (serialCount == 14) serialCount = 0; 164 | } else { 165 | if (serialCount > 0 || ch == '$') { 166 | teapotPacket[serialCount++] = (char)ch; 167 | if (serialCount == 14) { 168 | serialCount = 0; // restart packet byte position 169 | 170 | // get quaternion from data packet 171 | q[0] = ((teapotPacket[2] << 8) | teapotPacket[3]) / 16384.0f; 172 | q[1] = ((teapotPacket[4] << 8) | teapotPacket[5]) / 16384.0f; 173 | q[2] = ((teapotPacket[6] << 8) | teapotPacket[7]) / 16384.0f; 174 | q[3] = ((teapotPacket[8] << 8) | teapotPacket[9]) / 16384.0f; 175 | for (int i = 0; i < 4; i++) if (q[i] >= 2) q[i] = -4 + q[i]; 176 | 177 | // set our toxilibs quaternion to new data 178 | quat.set(q[0], q[1], q[2], q[3]); 179 | 180 | /* 181 | // below calculations unnecessary for orientation only using toxilibs 182 | 183 | // calculate gravity vector 184 | gravity[0] = 2 * (q[1]*q[3] - q[0]*q[2]); 185 | gravity[1] = 2 * (q[0]*q[1] + q[2]*q[3]); 186 | gravity[2] = q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3]; 187 | 188 | // calculate Euler angles 189 | euler[0] = atan2(2*q[1]*q[2] - 2*q[0]*q[3], 2*q[0]*q[0] + 2*q[1]*q[1] - 1); 190 | euler[1] = -asin(2*q[1]*q[3] + 2*q[0]*q[2]); 191 | euler[2] = atan2(2*q[2]*q[3] - 2*q[0]*q[1], 2*q[0]*q[0] + 2*q[3]*q[3] - 1); 192 | 193 | // calculate yaw/pitch/roll angles 194 | ypr[0] = atan2(2*q[1]*q[2] - 2*q[0]*q[3], 2*q[0]*q[0] + 2*q[1]*q[1] - 1); 195 | ypr[1] = atan(gravity[0] / sqrt(gravity[1]*gravity[1] + gravity[2]*gravity[2])); 196 | ypr[2] = atan(gravity[1] / sqrt(gravity[0]*gravity[0] + gravity[2]*gravity[2])); 197 | 198 | // output various components for debugging 199 | //println("q:\t" + round(q[0]*100.0f)/100.0f + "\t" + round(q[1]*100.0f)/100.0f + "\t" + round(q[2]*100.0f)/100.0f + "\t" + round(q[3]*100.0f)/100.0f); 200 | //println("euler:\t" + euler[0]*180.0f/PI + "\t" + euler[1]*180.0f/PI + "\t" + euler[2]*180.0f/PI); 201 | //println("ypr:\t" + ypr[0]*180.0f/PI + "\t" + ypr[1]*180.0f/PI + "\t" + ypr[2]*180.0f/PI); 202 | */ 203 | } 204 | } 205 | } 206 | } 207 | } 208 | 209 | void drawCylinder(float topRadius, float bottomRadius, float tall, int sides) { 210 | float angle = 0; 211 | float angleIncrement = TWO_PI / sides; 212 | beginShape(QUAD_STRIP); 213 | for (int i = 0; i < sides + 1; ++i) { 214 | vertex(topRadius*cos(angle), 0, topRadius*sin(angle)); 215 | vertex(bottomRadius*cos(angle), tall, bottomRadius*sin(angle)); 216 | angle += angleIncrement; 217 | } 218 | endShape(); 219 | 220 | // If it is not a cone, draw the circular top cap 221 | if (topRadius != 0) { 222 | angle = 0; 223 | beginShape(TRIANGLE_FAN); 224 | 225 | // Center point 226 | vertex(0, 0, 0); 227 | for (int i = 0; i < sides + 1; i++) { 228 | vertex(topRadius * cos(angle), 0, topRadius * sin(angle)); 229 | angle += angleIncrement; 230 | } 231 | endShape(); 232 | } 233 | 234 | // If it is not a cone, draw the circular bottom cap 235 | if (bottomRadius != 0) { 236 | angle = 0; 237 | beginShape(TRIANGLE_FAN); 238 | 239 | // Center point 240 | vertex(0, tall, 0); 241 | for (int i = 0; i < sides + 1; i++) { 242 | vertex(bottomRadius * cos(angle), tall, bottomRadius * sin(angle)); 243 | angle += angleIncrement; 244 | } 245 | endShape(); 246 | } 247 | } 248 | -------------------------------------------------------------------------------- /dev/I2Cdev/I2Cdev/I2Cdev.h: -------------------------------------------------------------------------------- 1 | // I2Cdev library collection - Main I2C device class header file 2 | // Abstracts bit and byte I2C R/W functions into a convenient class 3 | // 6/9/2012 by Jeff Rowberg 4 | // 5 | // Changelog: 6 | // 2013-05-06 - add Francesco Ferrara's Fastwire v0.24 implementation with small modifications 7 | // 2013-05-05 - fix issue with writing bit values to words (Sasquatch/Farzanegan) 8 | // 2012-06-09 - fix major issue with reading > 32 bytes at a time with Arduino Wire 9 | // - add compiler warnings when using outdated or IDE or limited I2Cdev implementation 10 | // 2011-11-01 - fix write*Bits mask calculation (thanks sasquatch @ Arduino forums) 11 | // 2011-10-03 - added automatic Arduino version detection for ease of use 12 | // 2011-10-02 - added Gene Knight's NBWire TwoWire class implementation with small modifications 13 | // 2011-08-31 - added support for Arduino 1.0 Wire library (methods are different from 0.x) 14 | // 2011-08-03 - added optional timeout parameter to read* methods to easily change from default 15 | // 2011-08-02 - added support for 16-bit registers 16 | // - fixed incorrect Doxygen comments on some methods 17 | // - added timeout value for read operations (thanks mem @ Arduino forums) 18 | // 2011-07-30 - changed read/write function structures to return success or byte counts 19 | // - made all methods static for multi-device memory savings 20 | // 2011-07-28 - initial release 21 | 22 | /* ============================================ 23 | I2Cdev device library code is placed under the MIT license 24 | Copyright (c) 2013 Jeff Rowberg 25 | 26 | Permission is hereby granted, free of charge, to any person obtaining a copy 27 | of this software and associated documentation files (the "Software"), to deal 28 | in the Software without restriction, including without limitation the rights 29 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 30 | copies of the Software, and to permit persons to whom the Software is 31 | furnished to do so, subject to the following conditions: 32 | 33 | The above copyright notice and this permission notice shall be included in 34 | all copies or substantial portions of the Software. 35 | 36 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 37 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 38 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 39 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 40 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 41 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 42 | THE SOFTWARE. 43 | =============================================== 44 | */ 45 | 46 | #ifndef _I2CDEV_H_ 47 | #define _I2CDEV_H_ 48 | 49 | // ----------------------------------------------------------------------------- 50 | // I2C interface implementation setting 51 | // ----------------------------------------------------------------------------- 52 | #define I2CDEV_IMPLEMENTATION I2CDEV_ARDUINO_WIRE 53 | //#define I2CDEV_IMPLEMENTATION I2CDEV_BUILTIN_FASTWIRE 54 | 55 | // comment this out if you are using a non-optimal IDE/implementation setting 56 | // but want the compiler to shut up about it 57 | #define I2CDEV_IMPLEMENTATION_WARNINGS 58 | 59 | // ----------------------------------------------------------------------------- 60 | // I2C interface implementation options 61 | // ----------------------------------------------------------------------------- 62 | #define I2CDEV_ARDUINO_WIRE 1 // Wire object from Arduino 63 | #define I2CDEV_BUILTIN_NBWIRE 2 // Tweaked Wire object from Gene Knight's NBWire project 64 | // ^^^ NBWire implementation is still buggy w/some interrupts! 65 | #define I2CDEV_BUILTIN_FASTWIRE 3 // FastWire object from Francesco Ferrara's project 66 | #define I2CDEV_I2CMASTER_LIBRARY 4 // I2C object from DSSCircuits I2C-Master Library at https://github.com/DSSCircuits/I2C-Master-Library 67 | 68 | // ----------------------------------------------------------------------------- 69 | // Arduino-style "Serial.print" debug constant (uncomment to enable) 70 | // ----------------------------------------------------------------------------- 71 | //#define I2CDEV_SERIAL_DEBUG 72 | 73 | #ifdef ARDUINO 74 | #if ARDUINO < 100 75 | #include "WProgram.h" 76 | #else 77 | #include "Arduino.h" 78 | #endif 79 | #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE 80 | #include 81 | #endif 82 | #if I2CDEV_IMPLEMENTATION == I2CDEV_I2CMASTER_LIBRARY 83 | #include 84 | #endif 85 | #endif 86 | 87 | // 1000ms default read timeout (modify with "I2Cdev::readTimeout = [ms];") 88 | #define I2CDEV_DEFAULT_READ_TIMEOUT 1000 89 | 90 | class I2Cdev { 91 | public: 92 | I2Cdev(); 93 | 94 | static int8_t readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout); 95 | static int8_t readBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout); 96 | static int8_t readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout); 97 | static int8_t readBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout); 98 | static int8_t readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout); 99 | static int8_t readWord(uint8_t devAddr, uint8_t regAddr, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout); 100 | static int8_t readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout); 101 | static int8_t readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout); 102 | 103 | static bool writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data); 104 | static bool writeBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t data); 105 | static bool writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data); 106 | static bool writeBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t data); 107 | static bool writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data); 108 | static bool writeWord(uint8_t devAddr, uint8_t regAddr, uint16_t data); 109 | static bool writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data); 110 | static bool writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data); 111 | 112 | static uint16_t readTimeout; 113 | }; 114 | 115 | #if I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE 116 | ////////////////////// 117 | // FastWire 0.24 118 | // This is a library to help faster programs to read I2C devices. 119 | // Copyright(C) 2012 120 | // Francesco Ferrara 121 | ////////////////////// 122 | 123 | /* Master */ 124 | #define TW_START 0x08 125 | #define TW_REP_START 0x10 126 | 127 | /* Master Transmitter */ 128 | #define TW_MT_SLA_ACK 0x18 129 | #define TW_MT_SLA_NACK 0x20 130 | #define TW_MT_DATA_ACK 0x28 131 | #define TW_MT_DATA_NACK 0x30 132 | #define TW_MT_ARB_LOST 0x38 133 | 134 | /* Master Receiver */ 135 | #define TW_MR_ARB_LOST 0x38 136 | #define TW_MR_SLA_ACK 0x40 137 | #define TW_MR_SLA_NACK 0x48 138 | #define TW_MR_DATA_ACK 0x50 139 | #define TW_MR_DATA_NACK 0x58 140 | 141 | #define TW_OK 0 142 | #define TW_ERROR 1 143 | 144 | class Fastwire { 145 | private: 146 | static boolean waitInt(); 147 | 148 | public: 149 | static void setup(int khz, boolean pullup); 150 | static byte beginTransmission(byte device); 151 | static byte write(byte value); 152 | static byte writeBuf(byte device, byte address, byte *data, byte num); 153 | static byte readBuf(byte device, byte address, byte *data, byte num); 154 | static void reset(); 155 | static byte stop(); 156 | }; 157 | #endif 158 | 159 | #if I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE 160 | // NBWire implementation based heavily on code by Gene Knight 161 | // Originally posted on the Arduino forum at http://arduino.cc/forum/index.php/topic,70705.0.html 162 | // Originally offered to the i2cdevlib project at http://arduino.cc/forum/index.php/topic,68210.30.html 163 | 164 | #define NBWIRE_BUFFER_LENGTH 32 165 | 166 | class TwoWire { 167 | private: 168 | static uint8_t rxBuffer[]; 169 | static uint8_t rxBufferIndex; 170 | static uint8_t rxBufferLength; 171 | 172 | static uint8_t txAddress; 173 | static uint8_t txBuffer[]; 174 | static uint8_t txBufferIndex; 175 | static uint8_t txBufferLength; 176 | 177 | // static uint8_t transmitting; 178 | static void (*user_onRequest)(void); 179 | static void (*user_onReceive)(int); 180 | static void onRequestService(void); 181 | static void onReceiveService(uint8_t*, int); 182 | 183 | public: 184 | TwoWire(); 185 | void begin(); 186 | void begin(uint8_t); 187 | void begin(int); 188 | void beginTransmission(uint8_t); 189 | //void beginTransmission(int); 190 | uint8_t endTransmission(uint16_t timeout=0); 191 | void nbendTransmission(void (*function)(int)) ; 192 | uint8_t requestFrom(uint8_t, int, uint16_t timeout=0); 193 | //uint8_t requestFrom(int, int); 194 | void nbrequestFrom(uint8_t, int, void (*function)(int)); 195 | void send(uint8_t); 196 | void send(uint8_t*, uint8_t); 197 | //void send(int); 198 | void send(char*); 199 | uint8_t available(void); 200 | uint8_t receive(void); 201 | void onReceive(void (*)(int)); 202 | void onRequest(void (*)(void)); 203 | }; 204 | 205 | #define TWI_READY 0 206 | #define TWI_MRX 1 207 | #define TWI_MTX 2 208 | #define TWI_SRX 3 209 | #define TWI_STX 4 210 | 211 | #define TW_WRITE 0 212 | #define TW_READ 1 213 | 214 | #define TW_MT_SLA_NACK 0x20 215 | #define TW_MT_DATA_NACK 0x30 216 | 217 | #define CPU_FREQ 16000000L 218 | #define TWI_FREQ 100000L 219 | #define TWI_BUFFER_LENGTH 32 220 | 221 | /* TWI Status is in TWSR, in the top 5 bits: TWS7 - TWS3 */ 222 | 223 | #define TW_STATUS_MASK (_BV(TWS7)|_BV(TWS6)|_BV(TWS5)|_BV(TWS4)|_BV(TWS3)) 224 | #define TW_STATUS (TWSR & TW_STATUS_MASK) 225 | #define TW_START 0x08 226 | #define TW_REP_START 0x10 227 | #define TW_MT_SLA_ACK 0x18 228 | #define TW_MT_SLA_NACK 0x20 229 | #define TW_MT_DATA_ACK 0x28 230 | #define TW_MT_DATA_NACK 0x30 231 | #define TW_MT_ARB_LOST 0x38 232 | #define TW_MR_ARB_LOST 0x38 233 | #define TW_MR_SLA_ACK 0x40 234 | #define TW_MR_SLA_NACK 0x48 235 | #define TW_MR_DATA_ACK 0x50 236 | #define TW_MR_DATA_NACK 0x58 237 | #define TW_ST_SLA_ACK 0xA8 238 | #define TW_ST_ARB_LOST_SLA_ACK 0xB0 239 | #define TW_ST_DATA_ACK 0xB8 240 | #define TW_ST_DATA_NACK 0xC0 241 | #define TW_ST_LAST_DATA 0xC8 242 | #define TW_SR_SLA_ACK 0x60 243 | #define TW_SR_ARB_LOST_SLA_ACK 0x68 244 | #define TW_SR_GCALL_ACK 0x70 245 | #define TW_SR_ARB_LOST_GCALL_ACK 0x78 246 | #define TW_SR_DATA_ACK 0x80 247 | #define TW_SR_DATA_NACK 0x88 248 | #define TW_SR_GCALL_DATA_ACK 0x90 249 | #define TW_SR_GCALL_DATA_NACK 0x98 250 | #define TW_SR_STOP 0xA0 251 | #define TW_NO_INFO 0xF8 252 | #define TW_BUS_ERROR 0x00 253 | 254 | //#define _MMIO_BYTE(mem_addr) (*(volatile uint8_t *)(mem_addr)) 255 | //#define _SFR_BYTE(sfr) _MMIO_BYTE(_SFR_ADDR(sfr)) 256 | 257 | #ifndef sbi // set bit 258 | #define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit)) 259 | #endif // sbi 260 | 261 | #ifndef cbi // clear bit 262 | #define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit)) 263 | #endif // cbi 264 | 265 | extern TwoWire Wire; 266 | 267 | #endif // I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE 268 | 269 | #endif /* _I2CDEV_H_ */ 270 | -------------------------------------------------------------------------------- /final/gyro_3719/gyro_3719.ino: -------------------------------------------------------------------------------- 1 | // I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class using DMP (MotionApps v2.0) 2 | // 6/21/2012 by Jeff Rowberg 3 | // Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib 4 | // 5 | // Changelog: 6 | // 2013-05-08 - added seamless Fastwire support 7 | // - added note about gyro calibration 8 | // 2012-06-21 - added note about Arduino 1.0.1 + Leonardo compatibility error 9 | // 2012-06-20 - improved FIFO overflow handling and simplified read process 10 | // 2012-06-19 - completely rearranged DMP initialization code and simplification 11 | // 2012-06-13 - pull gyro and accel data from FIFO packet instead of reading directly 12 | // 2012-06-09 - fix broken FIFO read sequence and change interrupt detection to RISING 13 | // 2012-06-05 - add gravity-compensated initial reference frame acceleration output 14 | // - add 3D math helper file to DMP6 example sketch 15 | // - add Euler output and Yaw/Pitch/Roll output formats 16 | // 2012-06-04 - remove accel offset clearing for better results (thanks Sungon Lee) 17 | // 2012-06-01 - fixed gyro sensitivity to be 2000 deg/sec instead of 250 18 | // 2012-05-30 - basic DMP initialization working 19 | 20 | /* ============================================ 21 | I2Cdev device library code is placed under the MIT license 22 | Copyright (c) 2012 Jeff Rowberg 23 | 24 | Permission is hereby granted, free of charge, to any person obtaining a copy 25 | of this software and associated documentation files (the "Software"), to deal 26 | in the Software without restriction, including without limitation the rights 27 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 28 | copies of the Software, and to permit persons to whom the Software is 29 | furnished to do so, subject to the following conditions: 30 | 31 | The above copyright notice and this permission notice shall be included in 32 | all copies or substantial portions of the Software. 33 | 34 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 35 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 36 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 37 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 38 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 39 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 40 | THE SOFTWARE. 41 | =============================================== 42 | */ 43 | 44 | //for the robotic arm implementation 45 | #include 46 | 47 | // I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files 48 | // for both classes must be in the include path of your project 49 | #include "I2Cdev.h" 50 | 51 | #include "MPU6050_6Axis_MotionApps20.h" 52 | //#include "MPU6050.h" // not necessary if using MotionApps include file 53 | 54 | // Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation 55 | // is used in I2Cdev.h 56 | #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE 57 | #include "Wire.h" 58 | #endif 59 | 60 | // class default I2C address is 0x68 61 | // specific I2C addresses may be passed as a parameter here 62 | // AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board) 63 | // AD0 high = 0x69 64 | MPU6050 mpu; 65 | //MPU6050 mpu(0x69); // <-- use for AD0 high 66 | 67 | /* ========================================================================= 68 | NOTE: In addition to connection 3.3v, GND, SDA, and SCL, this sketch 69 | depends on the MPU-6050's INT pin being connected to the Arduino's 70 | external interrupt #0 pin. On the Arduino Uno and Mega 2560, this is 71 | digital I/O pin 2. 72 | * ========================================================================= */ 73 | 74 | /* ========================================================================= 75 | NOTE: Arduino v1.0.1 with the Leonardo board generates a compile error 76 | when using Serial.write(buf, len). The Teapot output uses this method. 77 | The solution requires a modification to the Arduino USBAPI.h file, which 78 | is fortunately simple, but annoying. This will be fixed in the next IDE 79 | release. For more info, see these links: 80 | 81 | http://arduino.cc/forum/index.php/topic,109987.0.html 82 | http://code.google.com/p/arduino/issues/detail?id=958 83 | * ========================================================================= */ 84 | 85 | 86 | // uncomment "OUTPUT_READABLE_YAWPITCHROLL" if you want to see the yaw/ 87 | // pitch/roll angles (in degrees) calculated from the quaternions coming 88 | // from the FIFO. Note this also requires gravity vector calculations. 89 | // Also note that yaw/pitch/roll angles suffer from gimbal lock (for 90 | // more info, see: http://en.wikipedia.org/wiki/Gimbal_lock) 91 | #define OUTPUT_READABLE_YAWPITCHROLL 92 | 93 | 94 | 95 | 96 | 97 | #define LED_PIN 13 // (Arduino is 13, Teensy is 11, Teensy++ is 6) 98 | bool blinkState = false; 99 | 100 | // MPU control/status vars 101 | bool dmpReady = false; // set true if DMP init was successful 102 | uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU 103 | uint8_t devStatus; // return status after each device operation (0 = success, !0 = error) 104 | uint16_t packetSize; // expected DMP packet size (default is 42 bytes) 105 | uint16_t fifoCount; // count of all bytes currently in FIFO 106 | uint8_t fifoBuffer[64]; // FIFO storage buffer 107 | 108 | // orientation/motion vars 109 | Quaternion q; // [w, x, y, z] quaternion container 110 | VectorInt16 aa; // [x, y, z] accel sensor measurements 111 | VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements 112 | VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements 113 | VectorFloat gravity; // [x, y, z] gravity vector 114 | float euler[3]; // [psi, theta, phi] Euler angle container 115 | float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector 116 | 117 | // packet structure for InvenSense teapot demo 118 | uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' }; 119 | 120 | //servo initializations here 121 | 122 | #define baseServoPin 3 123 | #define shoulderServoPin 5 124 | #define elbowServoPin 6 125 | #define gripperServoPin 9 126 | #define FLEX_PIN A1 127 | #define FLEX_PIN2 A0 128 | 129 | // Initializes all of the needed servo motors 130 | Servo baseServo; 131 | Servo shoulderServo; 132 | Servo elbowServo; 133 | Servo gripperServo; 134 | 135 | int roll; 136 | int pitch; 137 | 138 | int baseVal; 139 | int shoulderVal; 140 | int elbowVal; 141 | int gripperVal; 142 | 143 | // Measure the voltage at 5V and the actual resistance of your 144 | // 47k resistor, and enter them below: 145 | const float VCC = 4.98; // Measured voltage of Ardunio 5V line 146 | const float R_DIV = 330.0; // Measured resistance of 3.3k resistor 147 | 148 | // Upload the code, then try to adjust these values to more 149 | // accurately calculate bend degree. 150 | const float STRAIGHT_RESISTANCE = 12000.0; // resistance when straight 151 | const float BEND_RESISTANCE = 50000.0; // resistance at 90 deg 152 | 153 | 154 | 155 | 156 | 157 | // ================================================================ 158 | // === INTERRUPT DETECTION ROUTINE === 159 | // ================================================================ 160 | 161 | volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high 162 | void dmpDataReady() { 163 | mpuInterrupt = true; 164 | } 165 | 166 | 167 | 168 | // ================================================================ 169 | // === INITIAL SETUP === 170 | // ================================================================ 171 | 172 | void setup() { 173 | 174 | //SET UP FOR FLEXORS 175 | pinMode(FLEX_PIN, INPUT); 176 | pinMode(FLEX_PIN2, INPUT); 177 | 178 | 179 | 180 | // Declared the pin attachments of all servo motors 181 | baseServo.attach(baseServoPin); 182 | shoulderServo.attach(shoulderServoPin); 183 | elbowServo.attach(elbowServoPin); 184 | gripperServo.attach(gripperServoPin); 185 | 186 | 187 | // join I2C bus (I2Cdev library doesn't do this automatically) 188 | #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE 189 | Wire.begin(); 190 | TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz) 191 | #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE 192 | Fastwire::setup(200, true); 193 | #endif 194 | 195 | // initialize serial communication 196 | // (115200 chosen because it is required for Teapot Demo output, but it's 197 | // really up to you depending on your project) 198 | Serial.begin(9600); 199 | while (!Serial); // wait for Leonardo enumeration, others continue immediately 200 | 201 | // NOTE: 8MHz or slower host processors, like the Teensy @ 3.3v or Ardunio 202 | // Pro Mini running at 3.3v, cannot handle this baud rate reliably due to 203 | // the baud timing being too misaligned with processor ticks. You must use 204 | // 38400 or slower in these cases, or use some kind of external separate 205 | // crystal solution for the UART timer. 206 | 207 | // initialize device 208 | Serial.println(F("Initializing I2C devices...")); 209 | mpu.initialize(); 210 | 211 | // verify connection 212 | Serial.println(F("Testing device connections...")); 213 | Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed")); 214 | 215 | // wait for ready 216 | Serial.println(F("\nSend any character to begin DMP programming and demo: ")); 217 | while (Serial.available() && Serial.read()); // empty buffer 218 | while (!Serial.available()); // wait for data 219 | while (Serial.available() && Serial.read()); // empty buffer again 220 | 221 | // load and configure the DMP 222 | Serial.println(F("Initializing DMP...")); 223 | devStatus = mpu.dmpInitialize(); 224 | 225 | // supply your own gyro offsets here, scaled for min sensitivity 226 | mpu.setXGyroOffset(220); 227 | mpu.setYGyroOffset(76); 228 | mpu.setZGyroOffset(-85); 229 | mpu.setZAccelOffset(1788); // 1688 factory default for my test chip 230 | 231 | // make sure it worked (returns 0 if so) 232 | if (devStatus == 0) { 233 | // turn on the DMP, now that it's ready 234 | Serial.println(F("Enabling DMP...")); 235 | mpu.setDMPEnabled(true); 236 | 237 | // enable Arduino interrupt detection 238 | Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)...")); 239 | attachInterrupt(0, dmpDataReady, RISING); 240 | mpuIntStatus = mpu.getIntStatus(); 241 | 242 | // set our DMP Ready flag so the main loop() function knows it's okay to use it 243 | Serial.println(F("DMP ready! Waiting for first interrupt...")); 244 | dmpReady = true; 245 | 246 | // get expected DMP packet size for later comparison 247 | packetSize = mpu.dmpGetFIFOPacketSize(); 248 | } else { 249 | // ERROR! 250 | // 1 = initial memory load failed 251 | // 2 = DMP configuration updates failed 252 | // (if it's going to break, usually the code will be 1) 253 | Serial.print(F("DMP Initialization failed (code ")); 254 | Serial.print(devStatus); 255 | Serial.println(F(")")); 256 | } 257 | 258 | // configure LED for output 259 | pinMode(LED_PIN, OUTPUT); 260 | } 261 | 262 | 263 | 264 | // ================================================================ 265 | // === MAIN PROGRAM LOOP === 266 | // ================================================================ 267 | 268 | void loop() { 269 | // if programming failed, don't try to do anything 270 | if (!dmpReady) return; 271 | 272 | // wait for MPU interrupt or extra packet(s) available 273 | while (!mpuInterrupt && fifoCount < packetSize) { 274 | // other program behavior stuff here 275 | // . 276 | // . 277 | // . 278 | // if you are really paranoid you can frequently test in between other 279 | // stuff to see if mpuInterrupt is true, and if so, "break;" from the 280 | // while() loop to immediately process the MPU data 281 | // . 282 | // . 283 | // . 284 | } 285 | 286 | // reset interrupt flag and get INT_STATUS byte 287 | mpuInterrupt = false; 288 | mpuIntStatus = mpu.getIntStatus(); 289 | 290 | // get current FIFO count 291 | fifoCount = mpu.getFIFOCount(); 292 | 293 | // check for overflow (this should never happen unless our code is too inefficient) 294 | if ((mpuIntStatus & 0x10) || fifoCount == 1024) { 295 | // reset so we can continue cleanly 296 | mpu.resetFIFO(); 297 | Serial.println(F("FIFO overflow!")); 298 | 299 | // otherwise, check for DMP data ready interrupt (this should happen frequently) 300 | } else if (mpuIntStatus & 0x02) { 301 | // wait for correct available data length, should be a VERY short wait 302 | while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount(); 303 | 304 | // read a packet from FIFO 305 | mpu.getFIFOBytes(fifoBuffer, packetSize); 306 | 307 | // track FIFO count here in case there is > 1 packet available 308 | // (this lets us immediately read more without waiting for an interrupt) 309 | fifoCount -= packetSize; 310 | 311 | /* 312 | * YOU NEED THIS FOR YPR READ OUTS @SANDEEP 313 | */ 314 | #ifdef OUTPUT_READABLE_YAWPITCHROLL 315 | // display Euler angles in degrees 316 | mpu.dmpGetQuaternion(&q, fifoBuffer); 317 | mpu.dmpGetGravity(&gravity, &q); 318 | mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); 319 | Serial.print("ypr\t"); 320 | Serial.print(ypr[0] * 180/M_PI); 321 | Serial.print("\t"); 322 | pitch = ypr[1] * 180/M_PI; 323 | Serial.print(pitch); 324 | Serial.print("\t"); 325 | roll = ypr[2] * 180/M_PI; 326 | Serial.println(roll); 327 | 328 | #endif 329 | // blink LED to indicate activity 330 | blinkState = !blinkState; 331 | digitalWrite(LED_PIN, blinkState); 332 | 333 | baseVal = map(roll, -77, 76, 90, 160); 334 | 335 | gripperVal = map(pitch, -69, 56, 5, 160); 336 | gripperServo.write(gripperVal); // Set servo position according to scaled value 337 | Serial.println("GRIPPER VAL: "); 338 | Serial.println(gripperVal); 339 | 340 | //Serial.println("\n"); 341 | //Serial.println("MAPPED VALUE: "); 342 | //Serial.println(baseVal); 343 | baseServo.write(baseVal); // Set servo position according to scaled value 344 | Serial.println("BASE VAL: "); 345 | Serial.println(baseVal); 346 | 347 | //flexors here 348 | // Read the ADC, and calculate voltage and resistance from it 349 | int flexADC = analogRead(FLEX_PIN); 350 | float flexV = flexADC * VCC / 1023.0; 351 | float flexR = R_DIV * (VCC / flexV - 1.0); 352 | 353 | //Flexor 2 354 | int flexADC2 = analogRead(FLEX_PIN2); 355 | float flexV2 = flexADC2 * VCC / 1023.0; 356 | float flexR2 = R_DIV * (VCC / flexV2 - 1.0); 357 | 358 | //Serial.println("SHOULDER Resistance: " + String(flexR) + " ohms"); 359 | Serial.println("ELBOW Resistance: " + String(flexR2) + " ohms"); 360 | 361 | // Use the calculated resistance to estimate the sensor's 362 | // bend angle: 363 | float angle = map(flexR, STRAIGHT_RESISTANCE, BEND_RESISTANCE, 364 | 30, 120.0); 365 | float angle2 = map(flexR2, STRAIGHT_RESISTANCE, BEND_RESISTANCE, 366 | 50, 140.0); 367 | 368 | Serial.println("SHOULDER Bend: " + String(angle) + " degrees"); 369 | Serial.println(); 370 | Serial.println("ELBOW Bend: " + String(angle2) + " degrees"); 371 | 372 | shoulderServo.write(angle); 373 | elbowServo.write(angle2); 374 | } 375 | 376 | 377 | /* 378 | shoulderVal = map(analogRead(shoulderPot), 260, 1023, 0, 175); 379 | shoulderServo.write(shoulderPotVal); 380 | 381 | elbowPotVal = map(analogRead(elbowPot), 0, 1023, 5, 175); 382 | elbowServo.write(elbowPotVal); 383 | 384 | gripperPotVal = map(analogRead(gripperPot), 0, 1023, 0, 180); 385 | gripperServo.write(gripperPotVal); 386 | 387 | */ 388 | 389 | 390 | } 391 | -------------------------------------------------------------------------------- /dev/MPU6050/Examples/MPU6050_DMP6/MPU6050_DMP6.ino: -------------------------------------------------------------------------------- 1 | // I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class using DMP (MotionApps v2.0) 2 | // 6/21/2012 by Jeff Rowberg 3 | // Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib 4 | // 5 | // Changelog: 6 | // 2013-05-08 - added seamless Fastwire support 7 | // - added note about gyro calibration 8 | // 2012-06-21 - added note about Arduino 1.0.1 + Leonardo compatibility error 9 | // 2012-06-20 - improved FIFO overflow handling and simplified read process 10 | // 2012-06-19 - completely rearranged DMP initialization code and simplification 11 | // 2012-06-13 - pull gyro and accel data from FIFO packet instead of reading directly 12 | // 2012-06-09 - fix broken FIFO read sequence and change interrupt detection to RISING 13 | // 2012-06-05 - add gravity-compensated initial reference frame acceleration output 14 | // - add 3D math helper file to DMP6 example sketch 15 | // - add Euler output and Yaw/Pitch/Roll output formats 16 | // 2012-06-04 - remove accel offset clearing for better results (thanks Sungon Lee) 17 | // 2012-06-01 - fixed gyro sensitivity to be 2000 deg/sec instead of 250 18 | // 2012-05-30 - basic DMP initialization working 19 | 20 | /* ============================================ 21 | I2Cdev device library code is placed under the MIT license 22 | Copyright (c) 2012 Jeff Rowberg 23 | 24 | Permission is hereby granted, free of charge, to any person obtaining a copy 25 | of this software and associated documentation files (the "Software"), to deal 26 | in the Software without restriction, including without limitation the rights 27 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 28 | copies of the Software, and to permit persons to whom the Software is 29 | furnished to do so, subject to the following conditions: 30 | 31 | The above copyright notice and this permission notice shall be included in 32 | all copies or substantial portions of the Software. 33 | 34 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 35 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 36 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 37 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 38 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 39 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 40 | THE SOFTWARE. 41 | =============================================== 42 | */ 43 | 44 | // I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files 45 | // for both classes must be in the include path of your project 46 | #include "I2Cdev.h" 47 | 48 | #include "MPU6050_6Axis_MotionApps20.h" 49 | //#include "MPU6050.h" // not necessary if using MotionApps include file 50 | 51 | // Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation 52 | // is used in I2Cdev.h 53 | #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE 54 | #include "Wire.h" 55 | #endif 56 | 57 | // class default I2C address is 0x68 58 | // specific I2C addresses may be passed as a parameter here 59 | // AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board) 60 | // AD0 high = 0x69 61 | MPU6050 mpu; 62 | //MPU6050 mpu(0x69); // <-- use for AD0 high 63 | 64 | /* ========================================================================= 65 | NOTE: In addition to connection 3.3v, GND, SDA, and SCL, this sketch 66 | depends on the MPU-6050's INT pin being connected to the Arduino's 67 | external interrupt #0 pin. On the Arduino Uno and Mega 2560, this is 68 | digital I/O pin 2. 69 | * ========================================================================= */ 70 | 71 | /* ========================================================================= 72 | NOTE: Arduino v1.0.1 with the Leonardo board generates a compile error 73 | when using Serial.write(buf, len). The Teapot output uses this method. 74 | The solution requires a modification to the Arduino USBAPI.h file, which 75 | is fortunately simple, but annoying. This will be fixed in the next IDE 76 | release. For more info, see these links: 77 | 78 | http://arduino.cc/forum/index.php/topic,109987.0.html 79 | http://code.google.com/p/arduino/issues/detail?id=958 80 | * ========================================================================= */ 81 | 82 | 83 | 84 | // uncomment "OUTPUT_READABLE_QUATERNION" if you want to see the actual 85 | // quaternion components in a [w, x, y, z] format (not best for parsing 86 | // on a remote host such as Processing or something though) 87 | //#define OUTPUT_READABLE_QUATERNION 88 | 89 | // uncomment "OUTPUT_READABLE_EULER" if you want to see Euler angles 90 | // (in degrees) calculated from the quaternions coming from the FIFO. 91 | // Note that Euler angles suffer from gimbal lock (for more info, see 92 | // http://en.wikipedia.org/wiki/Gimbal_lock) 93 | //#define OUTPUT_READABLE_EULER 94 | 95 | // uncomment "OUTPUT_READABLE_YAWPITCHROLL" if you want to see the yaw/ 96 | // pitch/roll angles (in degrees) calculated from the quaternions coming 97 | // from the FIFO. Note this also requires gravity vector calculations. 98 | // Also note that yaw/pitch/roll angles suffer from gimbal lock (for 99 | // more info, see: http://en.wikipedia.org/wiki/Gimbal_lock) 100 | #define OUTPUT_READABLE_YAWPITCHROLL 101 | 102 | // uncomment "OUTPUT_READABLE_REALACCEL" if you want to see acceleration 103 | // components with gravity removed. This acceleration reference frame is 104 | // not compensated for orientation, so +X is always +X according to the 105 | // sensor, just without the effects of gravity. If you want acceleration 106 | // compensated for orientation, us OUTPUT_READABLE_WORLDACCEL instead. 107 | //#define OUTPUT_READABLE_REALACCEL 108 | 109 | // uncomment "OUTPUT_READABLE_WORLDACCEL" if you want to see acceleration 110 | // components with gravity removed and adjusted for the world frame of 111 | // reference (yaw is relative to initial orientation, since no magnetometer 112 | // is present in this case). Could be quite handy in some cases. 113 | //#define OUTPUT_READABLE_WORLDACCEL 114 | 115 | // uncomment "OUTPUT_TEAPOT" if you want output that matches the 116 | // format used for the InvenSense teapot demo 117 | //#define OUTPUT_TEAPOT 118 | 119 | 120 | 121 | #define LED_PIN 13 // (Arduino is 13, Teensy is 11, Teensy++ is 6) 122 | bool blinkState = false; 123 | 124 | // MPU control/status vars 125 | bool dmpReady = false; // set true if DMP init was successful 126 | uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU 127 | uint8_t devStatus; // return status after each device operation (0 = success, !0 = error) 128 | uint16_t packetSize; // expected DMP packet size (default is 42 bytes) 129 | uint16_t fifoCount; // count of all bytes currently in FIFO 130 | uint8_t fifoBuffer[64]; // FIFO storage buffer 131 | 132 | // orientation/motion vars 133 | Quaternion q; // [w, x, y, z] quaternion container 134 | VectorInt16 aa; // [x, y, z] accel sensor measurements 135 | VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements 136 | VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements 137 | VectorFloat gravity; // [x, y, z] gravity vector 138 | float euler[3]; // [psi, theta, phi] Euler angle container 139 | float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector 140 | 141 | // packet structure for InvenSense teapot demo 142 | uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' }; 143 | 144 | 145 | 146 | // ================================================================ 147 | // === INTERRUPT DETECTION ROUTINE === 148 | // ================================================================ 149 | 150 | volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high 151 | void dmpDataReady() { 152 | mpuInterrupt = true; 153 | } 154 | 155 | 156 | 157 | // ================================================================ 158 | // === INITIAL SETUP === 159 | // ================================================================ 160 | 161 | void setup() { 162 | // join I2C bus (I2Cdev library doesn't do this automatically) 163 | #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE 164 | Wire.begin(); 165 | TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz) 166 | #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE 167 | Fastwire::setup(400, true); 168 | #endif 169 | 170 | // initialize serial communication 171 | // (115200 chosen because it is required for Teapot Demo output, but it's 172 | // really up to you depending on your project) 173 | Serial.begin(115200); 174 | while (!Serial); // wait for Leonardo enumeration, others continue immediately 175 | 176 | // NOTE: 8MHz or slower host processors, like the Teensy @ 3.3v or Ardunio 177 | // Pro Mini running at 3.3v, cannot handle this baud rate reliably due to 178 | // the baud timing being too misaligned with processor ticks. You must use 179 | // 38400 or slower in these cases, or use some kind of external separate 180 | // crystal solution for the UART timer. 181 | 182 | // initialize device 183 | Serial.println(F("Initializing I2C devices...")); 184 | mpu.initialize(); 185 | 186 | // verify connection 187 | Serial.println(F("Testing device connections...")); 188 | Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed")); 189 | 190 | // wait for ready 191 | Serial.println(F("\nSend any character to begin DMP programming and demo: ")); 192 | while (Serial.available() && Serial.read()); // empty buffer 193 | while (!Serial.available()); // wait for data 194 | while (Serial.available() && Serial.read()); // empty buffer again 195 | 196 | // load and configure the DMP 197 | Serial.println(F("Initializing DMP...")); 198 | devStatus = mpu.dmpInitialize(); 199 | 200 | // supply your own gyro offsets here, scaled for min sensitivity 201 | mpu.setXGyroOffset(220); 202 | mpu.setYGyroOffset(76); 203 | mpu.setZGyroOffset(-85); 204 | mpu.setZAccelOffset(1788); // 1688 factory default for my test chip 205 | 206 | // make sure it worked (returns 0 if so) 207 | if (devStatus == 0) { 208 | // turn on the DMP, now that it's ready 209 | Serial.println(F("Enabling DMP...")); 210 | mpu.setDMPEnabled(true); 211 | 212 | // enable Arduino interrupt detection 213 | Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)...")); 214 | attachInterrupt(0, dmpDataReady, RISING); 215 | mpuIntStatus = mpu.getIntStatus(); 216 | 217 | // set our DMP Ready flag so the main loop() function knows it's okay to use it 218 | Serial.println(F("DMP ready! Waiting for first interrupt...")); 219 | dmpReady = true; 220 | 221 | // get expected DMP packet size for later comparison 222 | packetSize = mpu.dmpGetFIFOPacketSize(); 223 | } else { 224 | // ERROR! 225 | // 1 = initial memory load failed 226 | // 2 = DMP configuration updates failed 227 | // (if it's going to break, usually the code will be 1) 228 | Serial.print(F("DMP Initialization failed (code ")); 229 | Serial.print(devStatus); 230 | Serial.println(F(")")); 231 | } 232 | 233 | // configure LED for output 234 | pinMode(LED_PIN, OUTPUT); 235 | } 236 | 237 | 238 | 239 | // ================================================================ 240 | // === MAIN PROGRAM LOOP === 241 | // ================================================================ 242 | 243 | void loop() { 244 | // if programming failed, don't try to do anything 245 | if (!dmpReady) return; 246 | 247 | // wait for MPU interrupt or extra packet(s) available 248 | while (!mpuInterrupt && fifoCount < packetSize) { 249 | // other program behavior stuff here 250 | // . 251 | // . 252 | // . 253 | // if you are really paranoid you can frequently test in between other 254 | // stuff to see if mpuInterrupt is true, and if so, "break;" from the 255 | // while() loop to immediately process the MPU data 256 | // . 257 | // . 258 | // . 259 | } 260 | 261 | // reset interrupt flag and get INT_STATUS byte 262 | mpuInterrupt = false; 263 | mpuIntStatus = mpu.getIntStatus(); 264 | 265 | // get current FIFO count 266 | fifoCount = mpu.getFIFOCount(); 267 | 268 | // check for overflow (this should never happen unless our code is too inefficient) 269 | if ((mpuIntStatus & 0x10) || fifoCount == 1024) { 270 | // reset so we can continue cleanly 271 | mpu.resetFIFO(); 272 | Serial.println(F("FIFO overflow!")); 273 | 274 | // otherwise, check for DMP data ready interrupt (this should happen frequently) 275 | } else if (mpuIntStatus & 0x02) { 276 | // wait for correct available data length, should be a VERY short wait 277 | while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount(); 278 | 279 | // read a packet from FIFO 280 | mpu.getFIFOBytes(fifoBuffer, packetSize); 281 | 282 | // track FIFO count here in case there is > 1 packet available 283 | // (this lets us immediately read more without waiting for an interrupt) 284 | fifoCount -= packetSize; 285 | 286 | #ifdef OUTPUT_READABLE_QUATERNION 287 | // display quaternion values in easy matrix form: w x y z 288 | mpu.dmpGetQuaternion(&q, fifoBuffer); 289 | Serial.print("quat\t"); 290 | Serial.print(q.w); 291 | Serial.print("\t"); 292 | Serial.print(q.x); 293 | Serial.print("\t"); 294 | Serial.print(q.y); 295 | Serial.print("\t"); 296 | Serial.println(q.z); 297 | #endif 298 | 299 | #ifdef OUTPUT_READABLE_EULER 300 | // display Euler angles in degrees 301 | mpu.dmpGetQuaternion(&q, fifoBuffer); 302 | mpu.dmpGetEuler(euler, &q); 303 | Serial.print("euler\t"); 304 | Serial.print(euler[0] * 180/M_PI); 305 | Serial.print("\t"); 306 | Serial.print(euler[1] * 180/M_PI); 307 | Serial.print("\t"); 308 | Serial.println(euler[2] * 180/M_PI); 309 | #endif 310 | 311 | #ifdef OUTPUT_READABLE_YAWPITCHROLL 312 | // display Euler angles in degrees 313 | mpu.dmpGetQuaternion(&q, fifoBuffer); 314 | mpu.dmpGetGravity(&gravity, &q); 315 | mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); 316 | Serial.print("ypr\t"); 317 | Serial.print(ypr[0] * 180/M_PI); 318 | Serial.print("\t"); 319 | Serial.print(ypr[1] * 180/M_PI); 320 | Serial.print("\t"); 321 | Serial.println(ypr[2] * 180/M_PI); 322 | #endif 323 | 324 | #ifdef OUTPUT_READABLE_REALACCEL 325 | // display real acceleration, adjusted to remove gravity 326 | mpu.dmpGetQuaternion(&q, fifoBuffer); 327 | mpu.dmpGetAccel(&aa, fifoBuffer); 328 | mpu.dmpGetGravity(&gravity, &q); 329 | mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity); 330 | Serial.print("areal\t"); 331 | Serial.print(aaReal.x); 332 | Serial.print("\t"); 333 | Serial.print(aaReal.y); 334 | Serial.print("\t"); 335 | Serial.println(aaReal.z); 336 | #endif 337 | 338 | #ifdef OUTPUT_READABLE_WORLDACCEL 339 | // display initial world-frame acceleration, adjusted to remove gravity 340 | // and rotated based on known orientation from quaternion 341 | mpu.dmpGetQuaternion(&q, fifoBuffer); 342 | mpu.dmpGetAccel(&aa, fifoBuffer); 343 | mpu.dmpGetGravity(&gravity, &q); 344 | mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity); 345 | mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q); 346 | Serial.print("aworld\t"); 347 | Serial.print(aaWorld.x); 348 | Serial.print("\t"); 349 | Serial.print(aaWorld.y); 350 | Serial.print("\t"); 351 | Serial.println(aaWorld.z); 352 | #endif 353 | 354 | #ifdef OUTPUT_TEAPOT 355 | // display quaternion values in InvenSense Teapot demo format: 356 | teapotPacket[2] = fifoBuffer[0]; 357 | teapotPacket[3] = fifoBuffer[1]; 358 | teapotPacket[4] = fifoBuffer[4]; 359 | teapotPacket[5] = fifoBuffer[5]; 360 | teapotPacket[6] = fifoBuffer[8]; 361 | teapotPacket[7] = fifoBuffer[9]; 362 | teapotPacket[8] = fifoBuffer[12]; 363 | teapotPacket[9] = fifoBuffer[13]; 364 | Serial.write(teapotPacket, 14); 365 | teapotPacket[11]++; // packetCount, loops at 0xFF on purpose 366 | #endif 367 | 368 | // blink LED to indicate activity 369 | blinkState = !blinkState; 370 | digitalWrite(LED_PIN, blinkState); 371 | } 372 | } 373 | -------------------------------------------------------------------------------- /dev/MPU6050/MPU6050_6Axis_MotionApps20.h: -------------------------------------------------------------------------------- 1 | // I2Cdev library collection - MPU6050 I2C device class, 6-axis MotionApps 2.0 implementation 2 | // Based on InvenSense MPU-6050 register map document rev. 2.0, 5/19/2011 (RM-MPU-6000A-00) 3 | // 5/20/2013 by Jeff Rowberg 4 | // Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib 5 | // 6 | // Changelog: 7 | // ... - ongoing debug release 8 | 9 | /* ============================================ 10 | I2Cdev device library code is placed under the MIT license 11 | Copyright (c) 2012 Jeff Rowberg 12 | 13 | Permission is hereby granted, free of charge, to any person obtaining a copy 14 | of this software and associated documentation files (the "Software"), to deal 15 | in the Software without restriction, including without limitation the rights 16 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 17 | copies of the Software, and to permit persons to whom the Software is 18 | furnished to do so, subject to the following conditions: 19 | 20 | The above copyright notice and this permission notice shall be included in 21 | all copies or substantial portions of the Software. 22 | 23 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 24 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 25 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 26 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 27 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 28 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 29 | THE SOFTWARE. 30 | =============================================== 31 | */ 32 | 33 | #ifndef _MPU6050_6AXIS_MOTIONAPPS20_H_ 34 | #define _MPU6050_6AXIS_MOTIONAPPS20_H_ 35 | 36 | #include "I2Cdev.h" 37 | #include "helper_3dmath.h" 38 | 39 | // MotionApps 2.0 DMP implementation, built using the MPU-6050EVB evaluation board 40 | #define MPU6050_INCLUDE_DMP_MOTIONAPPS20 41 | 42 | #include "MPU6050.h" 43 | 44 | // Tom Carpenter's conditional PROGMEM code 45 | // http://forum.arduino.cc/index.php?topic=129407.0 46 | #ifndef __arm__ 47 | #include 48 | #else 49 | // Teensy 3.0 library conditional PROGMEM code from Paul Stoffregen 50 | #ifndef __PGMSPACE_H_ 51 | #define __PGMSPACE_H_ 1 52 | #include 53 | 54 | #define PROGMEM 55 | #define PGM_P const char * 56 | #define PSTR(str) (str) 57 | #define F(x) x 58 | 59 | typedef void prog_void; 60 | typedef char prog_char; 61 | typedef unsigned char prog_uchar; 62 | typedef int8_t prog_int8_t; 63 | typedef uint8_t prog_uint8_t; 64 | typedef int16_t prog_int16_t; 65 | typedef uint16_t prog_uint16_t; 66 | typedef int32_t prog_int32_t; 67 | typedef uint32_t prog_uint32_t; 68 | 69 | #define strcpy_P(dest, src) strcpy((dest), (src)) 70 | #define strcat_P(dest, src) strcat((dest), (src)) 71 | #define strcmp_P(a, b) strcmp((a), (b)) 72 | 73 | #define pgm_read_byte(addr) (*(const unsigned char *)(addr)) 74 | #define pgm_read_word(addr) (*(const unsigned short *)(addr)) 75 | #define pgm_read_dword(addr) (*(const unsigned long *)(addr)) 76 | #define pgm_read_float(addr) (*(const float *)(addr)) 77 | 78 | #define pgm_read_byte_near(addr) pgm_read_byte(addr) 79 | #define pgm_read_word_near(addr) pgm_read_word(addr) 80 | #define pgm_read_dword_near(addr) pgm_read_dword(addr) 81 | #define pgm_read_float_near(addr) pgm_read_float(addr) 82 | #define pgm_read_byte_far(addr) pgm_read_byte(addr) 83 | #define pgm_read_word_far(addr) pgm_read_word(addr) 84 | #define pgm_read_dword_far(addr) pgm_read_dword(addr) 85 | #define pgm_read_float_far(addr) pgm_read_float(addr) 86 | #endif 87 | #endif 88 | 89 | /* Source is from the InvenSense MotionApps v2 demo code. Original source is 90 | * unavailable, unless you happen to be amazing as decompiling binary by 91 | * hand (in which case, please contact me, and I'm totally serious). 92 | * 93 | * Also, I'd like to offer many, many thanks to Noah Zerkin for all of the 94 | * DMP reverse-engineering he did to help make this bit of wizardry 95 | * possible. 96 | */ 97 | 98 | // NOTE! Enabling DEBUG adds about 3.3kB to the flash program size. 99 | // Debug output is now working even on ATMega328P MCUs (e.g. Arduino Uno) 100 | // after moving string constants to flash memory storage using the F() 101 | // compiler macro (Arduino IDE 1.0+ required). 102 | 103 | //#define DEBUG 104 | #ifdef DEBUG 105 | #define DEBUG_PRINT(x) Serial.print(x) 106 | #define DEBUG_PRINTF(x, y) Serial.print(x, y) 107 | #define DEBUG_PRINTLN(x) Serial.println(x) 108 | #define DEBUG_PRINTLNF(x, y) Serial.println(x, y) 109 | #else 110 | #define DEBUG_PRINT(x) 111 | #define DEBUG_PRINTF(x, y) 112 | #define DEBUG_PRINTLN(x) 113 | #define DEBUG_PRINTLNF(x, y) 114 | #endif 115 | 116 | #define MPU6050_DMP_CODE_SIZE 1929 // dmpMemory[] 117 | #define MPU6050_DMP_CONFIG_SIZE 192 // dmpConfig[] 118 | #define MPU6050_DMP_UPDATES_SIZE 47 // dmpUpdates[] 119 | 120 | /* ================================================================================================ * 121 | | Default MotionApps v2.0 42-byte FIFO packet structure: | 122 | | | 123 | | [QUAT W][ ][QUAT X][ ][QUAT Y][ ][QUAT Z][ ][GYRO X][ ][GYRO Y][ ] | 124 | | 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 | 125 | | | 126 | | [GYRO Z][ ][ACC X ][ ][ACC Y ][ ][ACC Z ][ ][ ] | 127 | | 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 | 128 | * ================================================================================================ */ 129 | 130 | // this block of memory gets written to the MPU on start-up, and it seems 131 | // to be volatile memory, so it has to be done each time (it only takes ~1 132 | // second though) 133 | const unsigned char dmpMemory[MPU6050_DMP_CODE_SIZE] PROGMEM = { 134 | // bank 0, 256 bytes 135 | 0xFB, 0x00, 0x00, 0x3E, 0x00, 0x0B, 0x00, 0x36, 0x00, 0x01, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00, 136 | 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0xFA, 0x80, 0x00, 0x0B, 0x12, 0x82, 0x00, 0x01, 137 | 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 138 | 0x00, 0x28, 0x00, 0x00, 0xFF, 0xFF, 0x45, 0x81, 0xFF, 0xFF, 0xFA, 0x72, 0x00, 0x00, 0x00, 0x00, 139 | 0x00, 0x00, 0x03, 0xE8, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x7F, 0xFF, 0xFF, 0xFE, 0x80, 0x01, 140 | 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 141 | 0x00, 0x3E, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xCA, 0xE3, 0x09, 0x3E, 0x80, 0x00, 0x00, 142 | 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00, 143 | 0x41, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x0B, 0x2A, 0x00, 0x00, 0x16, 0x55, 0x00, 0x00, 0x21, 0x82, 144 | 0xFD, 0x87, 0x26, 0x50, 0xFD, 0x80, 0x00, 0x00, 0x00, 0x1F, 0x00, 0x00, 0x00, 0x05, 0x80, 0x00, 145 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 146 | 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x6F, 0x00, 0x02, 0x65, 0x32, 0x00, 0x00, 0x5E, 0xC0, 147 | 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 148 | 0xFB, 0x8C, 0x6F, 0x5D, 0xFD, 0x5D, 0x08, 0xD9, 0x00, 0x7C, 0x73, 0x3B, 0x00, 0x6C, 0x12, 0xCC, 149 | 0x32, 0x00, 0x13, 0x9D, 0x32, 0x00, 0xD0, 0xD6, 0x32, 0x00, 0x08, 0x00, 0x40, 0x00, 0x01, 0xF4, 150 | 0xFF, 0xE6, 0x80, 0x79, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0xD0, 0xD6, 0x00, 0x00, 0x27, 0x10, 151 | 152 | // bank 1, 256 bytes 153 | 0xFB, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 154 | 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x00, 155 | 0x00, 0x00, 0xFA, 0x36, 0xFF, 0xBC, 0x30, 0x8E, 0x00, 0x05, 0xFB, 0xF0, 0xFF, 0xD9, 0x5B, 0xC8, 156 | 0xFF, 0xD0, 0x9A, 0xBE, 0x00, 0x00, 0x10, 0xA9, 0xFF, 0xF4, 0x1E, 0xB2, 0x00, 0xCE, 0xBB, 0xF7, 157 | 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x04, 0x00, 0x02, 0x00, 0x02, 0x02, 0x00, 0x00, 0x0C, 158 | 0xFF, 0xC2, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0xCF, 0x80, 0x00, 0x40, 0x00, 0x00, 0x00, 159 | 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14, 160 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 161 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 162 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 163 | 0x00, 0x00, 0x00, 0x00, 0x03, 0x3F, 0x68, 0xB6, 0x79, 0x35, 0x28, 0xBC, 0xC6, 0x7E, 0xD1, 0x6C, 164 | 0x80, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0xB2, 0x6A, 0x00, 0x00, 0x00, 0x00, 165 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3F, 0xF0, 0x00, 0x00, 0x00, 0x30, 166 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 167 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 168 | 0x00, 0x00, 0x25, 0x4D, 0x00, 0x2F, 0x70, 0x6D, 0x00, 0x00, 0x05, 0xAE, 0x00, 0x0C, 0x02, 0xD0, 169 | 170 | // bank 2, 256 bytes 171 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 172 | 0x00, 0x00, 0x01, 0x00, 0x00, 0x44, 0x00, 0x00, 0x00, 0x00, 0x0C, 0x00, 0x00, 0x00, 0x01, 0x00, 173 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x54, 0x00, 0x00, 0xFF, 0xEF, 0x00, 0x00, 174 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 175 | 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 176 | 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 177 | 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 178 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 179 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 180 | 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 181 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 182 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 183 | 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 184 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 185 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 186 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 187 | 188 | // bank 3, 256 bytes 189 | 0xD8, 0xDC, 0xBA, 0xA2, 0xF1, 0xDE, 0xB2, 0xB8, 0xB4, 0xA8, 0x81, 0x91, 0xF7, 0x4A, 0x90, 0x7F, 190 | 0x91, 0x6A, 0xF3, 0xF9, 0xDB, 0xA8, 0xF9, 0xB0, 0xBA, 0xA0, 0x80, 0xF2, 0xCE, 0x81, 0xF3, 0xC2, 191 | 0xF1, 0xC1, 0xF2, 0xC3, 0xF3, 0xCC, 0xA2, 0xB2, 0x80, 0xF1, 0xC6, 0xD8, 0x80, 0xBA, 0xA7, 0xDF, 192 | 0xDF, 0xDF, 0xF2, 0xA7, 0xC3, 0xCB, 0xC5, 0xB6, 0xF0, 0x87, 0xA2, 0x94, 0x24, 0x48, 0x70, 0x3C, 193 | 0x95, 0x40, 0x68, 0x34, 0x58, 0x9B, 0x78, 0xA2, 0xF1, 0x83, 0x92, 0x2D, 0x55, 0x7D, 0xD8, 0xB1, 194 | 0xB4, 0xB8, 0xA1, 0xD0, 0x91, 0x80, 0xF2, 0x70, 0xF3, 0x70, 0xF2, 0x7C, 0x80, 0xA8, 0xF1, 0x01, 195 | 0xB0, 0x98, 0x87, 0xD9, 0x43, 0xD8, 0x86, 0xC9, 0x88, 0xBA, 0xA1, 0xF2, 0x0E, 0xB8, 0x97, 0x80, 196 | 0xF1, 0xA9, 0xDF, 0xDF, 0xDF, 0xAA, 0xDF, 0xDF, 0xDF, 0xF2, 0xAA, 0xC5, 0xCD, 0xC7, 0xA9, 0x0C, 197 | 0xC9, 0x2C, 0x97, 0x97, 0x97, 0x97, 0xF1, 0xA9, 0x89, 0x26, 0x46, 0x66, 0xB0, 0xB4, 0xBA, 0x80, 198 | 0xAC, 0xDE, 0xF2, 0xCA, 0xF1, 0xB2, 0x8C, 0x02, 0xA9, 0xB6, 0x98, 0x00, 0x89, 0x0E, 0x16, 0x1E, 199 | 0xB8, 0xA9, 0xB4, 0x99, 0x2C, 0x54, 0x7C, 0xB0, 0x8A, 0xA8, 0x96, 0x36, 0x56, 0x76, 0xF1, 0xB9, 200 | 0xAF, 0xB4, 0xB0, 0x83, 0xC0, 0xB8, 0xA8, 0x97, 0x11, 0xB1, 0x8F, 0x98, 0xB9, 0xAF, 0xF0, 0x24, 201 | 0x08, 0x44, 0x10, 0x64, 0x18, 0xF1, 0xA3, 0x29, 0x55, 0x7D, 0xAF, 0x83, 0xB5, 0x93, 0xAF, 0xF0, 202 | 0x00, 0x28, 0x50, 0xF1, 0xA3, 0x86, 0x9F, 0x61, 0xA6, 0xDA, 0xDE, 0xDF, 0xD9, 0xFA, 0xA3, 0x86, 203 | 0x96, 0xDB, 0x31, 0xA6, 0xD9, 0xF8, 0xDF, 0xBA, 0xA6, 0x8F, 0xC2, 0xC5, 0xC7, 0xB2, 0x8C, 0xC1, 204 | 0xB8, 0xA2, 0xDF, 0xDF, 0xDF, 0xA3, 0xDF, 0xDF, 0xDF, 0xD8, 0xD8, 0xF1, 0xB8, 0xA8, 0xB2, 0x86, 205 | 206 | // bank 4, 256 bytes 207 | 0xB4, 0x98, 0x0D, 0x35, 0x5D, 0xB8, 0xAA, 0x98, 0xB0, 0x87, 0x2D, 0x35, 0x3D, 0xB2, 0xB6, 0xBA, 208 | 0xAF, 0x8C, 0x96, 0x19, 0x8F, 0x9F, 0xA7, 0x0E, 0x16, 0x1E, 0xB4, 0x9A, 0xB8, 0xAA, 0x87, 0x2C, 209 | 0x54, 0x7C, 0xB9, 0xA3, 0xDE, 0xDF, 0xDF, 0xA3, 0xB1, 0x80, 0xF2, 0xC4, 0xCD, 0xC9, 0xF1, 0xB8, 210 | 0xA9, 0xB4, 0x99, 0x83, 0x0D, 0x35, 0x5D, 0x89, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0xB5, 0x93, 0xA3, 211 | 0x0E, 0x16, 0x1E, 0xA9, 0x2C, 0x54, 0x7C, 0xB8, 0xB4, 0xB0, 0xF1, 0x97, 0x83, 0xA8, 0x11, 0x84, 212 | 0xA5, 0x09, 0x98, 0xA3, 0x83, 0xF0, 0xDA, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xD8, 0xF1, 0xA5, 213 | 0x29, 0x55, 0x7D, 0xA5, 0x85, 0x95, 0x02, 0x1A, 0x2E, 0x3A, 0x56, 0x5A, 0x40, 0x48, 0xF9, 0xF3, 214 | 0xA3, 0xD9, 0xF8, 0xF0, 0x98, 0x83, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0x97, 0x82, 0xA8, 0xF1, 215 | 0x11, 0xF0, 0x98, 0xA2, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xDA, 0xF3, 0xDE, 0xD8, 0x83, 0xA5, 216 | 0x94, 0x01, 0xD9, 0xA3, 0x02, 0xF1, 0xA2, 0xC3, 0xC5, 0xC7, 0xD8, 0xF1, 0x84, 0x92, 0xA2, 0x4D, 217 | 0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9, 218 | 0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0x93, 0xA3, 0x4D, 219 | 0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9, 220 | 0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0xA8, 0x8A, 0x9A, 221 | 0xF0, 0x28, 0x50, 0x78, 0x9E, 0xF3, 0x88, 0x18, 0xF1, 0x9F, 0x1D, 0x98, 0xA8, 0xD9, 0x08, 0xD8, 222 | 0xC8, 0x9F, 0x12, 0x9E, 0xF3, 0x15, 0xA8, 0xDA, 0x12, 0x10, 0xD8, 0xF1, 0xAF, 0xC8, 0x97, 0x87, 223 | 224 | // bank 5, 256 bytes 225 | 0x34, 0xB5, 0xB9, 0x94, 0xA4, 0x21, 0xF3, 0xD9, 0x22, 0xD8, 0xF2, 0x2D, 0xF3, 0xD9, 0x2A, 0xD8, 226 | 0xF2, 0x35, 0xF3, 0xD9, 0x32, 0xD8, 0x81, 0xA4, 0x60, 0x60, 0x61, 0xD9, 0x61, 0xD8, 0x6C, 0x68, 227 | 0x69, 0xD9, 0x69, 0xD8, 0x74, 0x70, 0x71, 0xD9, 0x71, 0xD8, 0xB1, 0xA3, 0x84, 0x19, 0x3D, 0x5D, 228 | 0xA3, 0x83, 0x1A, 0x3E, 0x5E, 0x93, 0x10, 0x30, 0x81, 0x10, 0x11, 0xB8, 0xB0, 0xAF, 0x8F, 0x94, 229 | 0xF2, 0xDA, 0x3E, 0xD8, 0xB4, 0x9A, 0xA8, 0x87, 0x29, 0xDA, 0xF8, 0xD8, 0x87, 0x9A, 0x35, 0xDA, 230 | 0xF8, 0xD8, 0x87, 0x9A, 0x3D, 0xDA, 0xF8, 0xD8, 0xB1, 0xB9, 0xA4, 0x98, 0x85, 0x02, 0x2E, 0x56, 231 | 0xA5, 0x81, 0x00, 0x0C, 0x14, 0xA3, 0x97, 0xB0, 0x8A, 0xF1, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9, 232 | 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x84, 0x0D, 0xDA, 0x0E, 0xD8, 0xA3, 0x29, 0x83, 0xDA, 233 | 0x2C, 0x0E, 0xD8, 0xA3, 0x84, 0x49, 0x83, 0xDA, 0x2C, 0x4C, 0x0E, 0xD8, 0xB8, 0xB0, 0xA8, 0x8A, 234 | 0x9A, 0xF5, 0x20, 0xAA, 0xDA, 0xDF, 0xD8, 0xA8, 0x40, 0xAA, 0xD0, 0xDA, 0xDE, 0xD8, 0xA8, 0x60, 235 | 0xAA, 0xDA, 0xD0, 0xDF, 0xD8, 0xF1, 0x97, 0x86, 0xA8, 0x31, 0x9B, 0x06, 0x99, 0x07, 0xAB, 0x97, 236 | 0x28, 0x88, 0x9B, 0xF0, 0x0C, 0x20, 0x14, 0x40, 0xB8, 0xB0, 0xB4, 0xA8, 0x8C, 0x9C, 0xF0, 0x04, 237 | 0x28, 0x51, 0x79, 0x1D, 0x30, 0x14, 0x38, 0xB2, 0x82, 0xAB, 0xD0, 0x98, 0x2C, 0x50, 0x50, 0x78, 238 | 0x78, 0x9B, 0xF1, 0x1A, 0xB0, 0xF0, 0x8A, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x8B, 0x29, 0x51, 0x79, 239 | 0x8A, 0x24, 0x70, 0x59, 0x8B, 0x20, 0x58, 0x71, 0x8A, 0x44, 0x69, 0x38, 0x8B, 0x39, 0x40, 0x68, 240 | 0x8A, 0x64, 0x48, 0x31, 0x8B, 0x30, 0x49, 0x60, 0xA5, 0x88, 0x20, 0x09, 0x71, 0x58, 0x44, 0x68, 241 | 242 | // bank 6, 256 bytes 243 | 0x11, 0x39, 0x64, 0x49, 0x30, 0x19, 0xF1, 0xAC, 0x00, 0x2C, 0x54, 0x7C, 0xF0, 0x8C, 0xA8, 0x04, 244 | 0x28, 0x50, 0x78, 0xF1, 0x88, 0x97, 0x26, 0xA8, 0x59, 0x98, 0xAC, 0x8C, 0x02, 0x26, 0x46, 0x66, 245 | 0xF0, 0x89, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31, 246 | 0xA9, 0x88, 0x09, 0x20, 0x59, 0x70, 0xAB, 0x11, 0x38, 0x40, 0x69, 0xA8, 0x19, 0x31, 0x48, 0x60, 247 | 0x8C, 0xA8, 0x3C, 0x41, 0x5C, 0x20, 0x7C, 0x00, 0xF1, 0x87, 0x98, 0x19, 0x86, 0xA8, 0x6E, 0x76, 248 | 0x7E, 0xA9, 0x99, 0x88, 0x2D, 0x55, 0x7D, 0x9E, 0xB9, 0xA3, 0x8A, 0x22, 0x8A, 0x6E, 0x8A, 0x56, 249 | 0x8A, 0x5E, 0x9F, 0xB1, 0x83, 0x06, 0x26, 0x46, 0x66, 0x0E, 0x2E, 0x4E, 0x6E, 0x9D, 0xB8, 0xAD, 250 | 0x00, 0x2C, 0x54, 0x7C, 0xF2, 0xB1, 0x8C, 0xB4, 0x99, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0x81, 0x91, 251 | 0xAC, 0x38, 0xAD, 0x3A, 0xB5, 0x83, 0x91, 0xAC, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9, 0x48, 0xD8, 252 | 0x6D, 0xD9, 0x68, 0xD8, 0x8C, 0x9D, 0xAE, 0x29, 0xD9, 0x04, 0xAE, 0xD8, 0x51, 0xD9, 0x04, 0xAE, 253 | 0xD8, 0x79, 0xD9, 0x04, 0xD8, 0x81, 0xF3, 0x9D, 0xAD, 0x00, 0x8D, 0xAE, 0x19, 0x81, 0xAD, 0xD9, 254 | 0x01, 0xD8, 0xF2, 0xAE, 0xDA, 0x26, 0xD8, 0x8E, 0x91, 0x29, 0x83, 0xA7, 0xD9, 0xAD, 0xAD, 0xAD, 255 | 0xAD, 0xF3, 0x2A, 0xD8, 0xD8, 0xF1, 0xB0, 0xAC, 0x89, 0x91, 0x3E, 0x5E, 0x76, 0xF3, 0xAC, 0x2E, 256 | 0x2E, 0xF1, 0xB1, 0x8C, 0x5A, 0x9C, 0xAC, 0x2C, 0x28, 0x28, 0x28, 0x9C, 0xAC, 0x30, 0x18, 0xA8, 257 | 0x98, 0x81, 0x28, 0x34, 0x3C, 0x97, 0x24, 0xA7, 0x28, 0x34, 0x3C, 0x9C, 0x24, 0xF2, 0xB0, 0x89, 258 | 0xAC, 0x91, 0x2C, 0x4C, 0x6C, 0x8A, 0x9B, 0x2D, 0xD9, 0xD8, 0xD8, 0x51, 0xD9, 0xD8, 0xD8, 0x79, 259 | 260 | // bank 7, 138 bytes (remainder) 261 | 0xD9, 0xD8, 0xD8, 0xF1, 0x9E, 0x88, 0xA3, 0x31, 0xDA, 0xD8, 0xD8, 0x91, 0x2D, 0xD9, 0x28, 0xD8, 262 | 0x4D, 0xD9, 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x83, 0x93, 0x35, 0x3D, 0x80, 0x25, 0xDA, 263 | 0xD8, 0xD8, 0x85, 0x69, 0xDA, 0xD8, 0xD8, 0xB4, 0x93, 0x81, 0xA3, 0x28, 0x34, 0x3C, 0xF3, 0xAB, 264 | 0x8B, 0xF8, 0xA3, 0x91, 0xB6, 0x09, 0xB4, 0xD9, 0xAB, 0xDE, 0xFA, 0xB0, 0x87, 0x9C, 0xB9, 0xA3, 265 | 0xDD, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x95, 0xF1, 0xA3, 0xA3, 0xA3, 0x9D, 0xF1, 0xA3, 0xA3, 0xA3, 266 | 0xA3, 0xF2, 0xA3, 0xB4, 0x90, 0x80, 0xF2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 267 | 0xA3, 0xB2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xB0, 0x87, 0xB5, 0x99, 0xF1, 0xA3, 0xA3, 0xA3, 268 | 0x98, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x97, 0xA3, 0xA3, 0xA3, 0xA3, 0xF3, 0x9B, 0xA3, 0xA3, 0xDC, 269 | 0xB9, 0xA7, 0xF1, 0x26, 0x26, 0x26, 0xD8, 0xD8, 0xFF 270 | }; 271 | 272 | // thanks to Noah Zerkin for piecing this stuff together! 273 | const unsigned char dmpConfig[MPU6050_DMP_CONFIG_SIZE] PROGMEM = { 274 | // BANK OFFSET LENGTH [DATA] 275 | 0x03, 0x7B, 0x03, 0x4C, 0xCD, 0x6C, // FCFG_1 inv_set_gyro_calibration 276 | 0x03, 0xAB, 0x03, 0x36, 0x56, 0x76, // FCFG_3 inv_set_gyro_calibration 277 | 0x00, 0x68, 0x04, 0x02, 0xCB, 0x47, 0xA2, // D_0_104 inv_set_gyro_calibration 278 | 0x02, 0x18, 0x04, 0x00, 0x05, 0x8B, 0xC1, // D_0_24 inv_set_gyro_calibration 279 | 0x01, 0x0C, 0x04, 0x00, 0x00, 0x00, 0x00, // D_1_152 inv_set_accel_calibration 280 | 0x03, 0x7F, 0x06, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97, // FCFG_2 inv_set_accel_calibration 281 | 0x03, 0x89, 0x03, 0x26, 0x46, 0x66, // FCFG_7 inv_set_accel_calibration 282 | 0x00, 0x6C, 0x02, 0x20, 0x00, // D_0_108 inv_set_accel_calibration 283 | 0x02, 0x40, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_00 inv_set_compass_calibration 284 | 0x02, 0x44, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_01 285 | 0x02, 0x48, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_02 286 | 0x02, 0x4C, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_10 287 | 0x02, 0x50, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_11 288 | 0x02, 0x54, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_12 289 | 0x02, 0x58, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_20 290 | 0x02, 0x5C, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_21 291 | 0x02, 0xBC, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_22 292 | 0x01, 0xEC, 0x04, 0x00, 0x00, 0x40, 0x00, // D_1_236 inv_apply_endian_accel 293 | 0x03, 0x7F, 0x06, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97, // FCFG_2 inv_set_mpu_sensors 294 | 0x04, 0x02, 0x03, 0x0D, 0x35, 0x5D, // CFG_MOTION_BIAS inv_turn_on_bias_from_no_motion 295 | 0x04, 0x09, 0x04, 0x87, 0x2D, 0x35, 0x3D, // FCFG_5 inv_set_bias_update 296 | 0x00, 0xA3, 0x01, 0x00, // D_0_163 inv_set_dead_zone 297 | // SPECIAL 0x01 = enable interrupts 298 | 0x00, 0x00, 0x00, 0x01, // SET INT_ENABLE at i=22, SPECIAL INSTRUCTION 299 | 0x07, 0x86, 0x01, 0xFE, // CFG_6 inv_set_fifo_interupt 300 | 0x07, 0x41, 0x05, 0xF1, 0x20, 0x28, 0x30, 0x38, // CFG_8 inv_send_quaternion 301 | 0x07, 0x7E, 0x01, 0x30, // CFG_16 inv_set_footer 302 | 0x07, 0x46, 0x01, 0x9A, // CFG_GYRO_SOURCE inv_send_gyro 303 | 0x07, 0x47, 0x04, 0xF1, 0x28, 0x30, 0x38, // CFG_9 inv_send_gyro -> inv_construct3_fifo 304 | 0x07, 0x6C, 0x04, 0xF1, 0x28, 0x30, 0x38, // CFG_12 inv_send_accel -> inv_construct3_fifo 305 | 0x02, 0x16, 0x02, 0x00, 0x02 //(0x07 -> 16Mhz) D_0_22 inv_set_fifo_rate (0x06 for first 8mhz board) (0x09 for 8Mhz board from Binoy) 306 | 307 | // This very last 0x01 WAS a 0x09, which drops the FIFO rate down to 20 Hz. 0x07 is 25 Hz, 308 | // 0x01 is 100Hz. Going faster than 100Hz (0x00=200Hz) tends to result in very noisy data. 309 | // DMP output frequency is calculated easily using this equation: (200Hz / (1 + value)) 310 | 311 | // It is important to make sure the host processor can keep up with reading and processing 312 | // the FIFO output at the desired rate. Handling FIFO overflow cleanly is also a good idea. 313 | }; 314 | 315 | const unsigned char dmpUpdates[MPU6050_DMP_UPDATES_SIZE] PROGMEM = { 316 | 0x01, 0xB2, 0x02, 0xFF, 0xFF, 317 | 0x01, 0x90, 0x04, 0x09, 0x23, 0xA1, 0x35, 318 | 0x01, 0x6A, 0x02, 0x06, 0x00, 319 | 0x01, 0x60, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 320 | 0x00, 0x60, 0x04, 0x40, 0x00, 0x00, 0x00, 321 | 0x01, 0x62, 0x02, 0x00, 0x00, 322 | 0x00, 0x60, 0x04, 0x00, 0x40, 0x00, 0x00 323 | }; 324 | 325 | uint8_t MPU6050::dmpInitialize() { 326 | // reset device 327 | DEBUG_PRINTLN(F("\n\nResetting MPU6050...")); 328 | reset(); 329 | delay(30); // wait after reset 330 | 331 | // enable sleep mode and wake cycle 332 | /*Serial.println(F("Enabling sleep mode...")); 333 | setSleepEnabled(true); 334 | Serial.println(F("Enabling wake cycle...")); 335 | setWakeCycleEnabled(true);*/ 336 | 337 | // disable sleep mode 338 | DEBUG_PRINTLN(F("Disabling sleep mode...")); 339 | setSleepEnabled(false); 340 | 341 | // get MPU hardware revision 342 | DEBUG_PRINTLN(F("Selecting user bank 16...")); 343 | setMemoryBank(0x10, true, true); 344 | DEBUG_PRINTLN(F("Selecting memory byte 6...")); 345 | setMemoryStartAddress(0x06); 346 | DEBUG_PRINTLN(F("Checking hardware revision...")); 347 | uint8_t hwRevision = readMemoryByte(); 348 | DEBUG_PRINT(F("Revision @ user[16][6] = ")); 349 | DEBUG_PRINTLNF(hwRevision, HEX); 350 | DEBUG_PRINTLN(F("Resetting memory bank selection to 0...")); 351 | setMemoryBank(0, false, false); 352 | 353 | // check OTP bank valid 354 | DEBUG_PRINTLN(F("Reading OTP bank valid flag...")); 355 | uint8_t otpValid = getOTPBankValid(); 356 | DEBUG_PRINT(F("OTP bank is ")); 357 | DEBUG_PRINTLN(otpValid ? F("valid!") : F("invalid!")); 358 | 359 | // get X/Y/Z gyro offsets 360 | DEBUG_PRINTLN(F("Reading gyro offset TC values...")); 361 | int8_t xgOffsetTC = getXGyroOffsetTC(); 362 | int8_t ygOffsetTC = getYGyroOffsetTC(); 363 | int8_t zgOffsetTC = getZGyroOffsetTC(); 364 | DEBUG_PRINT(F("X gyro offset = ")); 365 | DEBUG_PRINTLN(xgOffset); 366 | DEBUG_PRINT(F("Y gyro offset = ")); 367 | DEBUG_PRINTLN(ygOffset); 368 | DEBUG_PRINT(F("Z gyro offset = ")); 369 | DEBUG_PRINTLN(zgOffset); 370 | 371 | // setup weird slave stuff (?) 372 | DEBUG_PRINTLN(F("Setting slave 0 address to 0x7F...")); 373 | setSlaveAddress(0, 0x7F); 374 | DEBUG_PRINTLN(F("Disabling I2C Master mode...")); 375 | setI2CMasterModeEnabled(false); 376 | DEBUG_PRINTLN(F("Setting slave 0 address to 0x68 (self)...")); 377 | setSlaveAddress(0, 0x68); 378 | DEBUG_PRINTLN(F("Resetting I2C Master control...")); 379 | resetI2CMaster(); 380 | delay(20); 381 | 382 | // load DMP code into memory banks 383 | DEBUG_PRINT(F("Writing DMP code to MPU memory banks (")); 384 | DEBUG_PRINT(MPU6050_DMP_CODE_SIZE); 385 | DEBUG_PRINTLN(F(" bytes)")); 386 | if (writeProgMemoryBlock(dmpMemory, MPU6050_DMP_CODE_SIZE)) { 387 | DEBUG_PRINTLN(F("Success! DMP code written and verified.")); 388 | 389 | // write DMP configuration 390 | DEBUG_PRINT(F("Writing DMP configuration to MPU memory banks (")); 391 | DEBUG_PRINT(MPU6050_DMP_CONFIG_SIZE); 392 | DEBUG_PRINTLN(F(" bytes in config def)")); 393 | if (writeProgDMPConfigurationSet(dmpConfig, MPU6050_DMP_CONFIG_SIZE)) { 394 | DEBUG_PRINTLN(F("Success! DMP configuration written and verified.")); 395 | 396 | DEBUG_PRINTLN(F("Setting clock source to Z Gyro...")); 397 | setClockSource(MPU6050_CLOCK_PLL_ZGYRO); 398 | 399 | DEBUG_PRINTLN(F("Setting DMP and FIFO_OFLOW interrupts enabled...")); 400 | setIntEnabled(0x12); 401 | 402 | DEBUG_PRINTLN(F("Setting sample rate to 200Hz...")); 403 | setRate(4); // 1khz / (1 + 4) = 200 Hz 404 | 405 | DEBUG_PRINTLN(F("Setting external frame sync to TEMP_OUT_L[0]...")); 406 | setExternalFrameSync(MPU6050_EXT_SYNC_TEMP_OUT_L); 407 | 408 | DEBUG_PRINTLN(F("Setting DLPF bandwidth to 42Hz...")); 409 | setDLPFMode(MPU6050_DLPF_BW_42); 410 | 411 | DEBUG_PRINTLN(F("Setting gyro sensitivity to +/- 2000 deg/sec...")); 412 | setFullScaleGyroRange(MPU6050_GYRO_FS_2000); 413 | 414 | DEBUG_PRINTLN(F("Setting DMP configuration bytes (function unknown)...")); 415 | setDMPConfig1(0x03); 416 | setDMPConfig2(0x00); 417 | 418 | DEBUG_PRINTLN(F("Clearing OTP Bank flag...")); 419 | setOTPBankValid(false); 420 | 421 | DEBUG_PRINTLN(F("Setting X/Y/Z gyro offset TCs to previous values...")); 422 | setXGyroOffsetTC(xgOffsetTC); 423 | setYGyroOffsetTC(ygOffsetTC); 424 | setZGyroOffsetTC(zgOffsetTC); 425 | 426 | //DEBUG_PRINTLN(F("Setting X/Y/Z gyro user offsets to zero...")); 427 | //setXGyroOffset(0); 428 | //setYGyroOffset(0); 429 | //setZGyroOffset(0); 430 | 431 | DEBUG_PRINTLN(F("Writing final memory update 1/7 (function unknown)...")); 432 | uint8_t dmpUpdate[16], j; 433 | uint16_t pos = 0; 434 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 435 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 436 | 437 | DEBUG_PRINTLN(F("Writing final memory update 2/7 (function unknown)...")); 438 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 439 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 440 | 441 | DEBUG_PRINTLN(F("Resetting FIFO...")); 442 | resetFIFO(); 443 | 444 | DEBUG_PRINTLN(F("Reading FIFO count...")); 445 | uint16_t fifoCount = getFIFOCount(); 446 | uint8_t fifoBuffer[128]; 447 | 448 | DEBUG_PRINT(F("Current FIFO count=")); 449 | DEBUG_PRINTLN(fifoCount); 450 | getFIFOBytes(fifoBuffer, fifoCount); 451 | 452 | DEBUG_PRINTLN(F("Setting motion detection threshold to 2...")); 453 | setMotionDetectionThreshold(2); 454 | 455 | DEBUG_PRINTLN(F("Setting zero-motion detection threshold to 156...")); 456 | setZeroMotionDetectionThreshold(156); 457 | 458 | DEBUG_PRINTLN(F("Setting motion detection duration to 80...")); 459 | setMotionDetectionDuration(80); 460 | 461 | DEBUG_PRINTLN(F("Setting zero-motion detection duration to 0...")); 462 | setZeroMotionDetectionDuration(0); 463 | 464 | DEBUG_PRINTLN(F("Resetting FIFO...")); 465 | resetFIFO(); 466 | 467 | DEBUG_PRINTLN(F("Enabling FIFO...")); 468 | setFIFOEnabled(true); 469 | 470 | DEBUG_PRINTLN(F("Enabling DMP...")); 471 | setDMPEnabled(true); 472 | 473 | DEBUG_PRINTLN(F("Resetting DMP...")); 474 | resetDMP(); 475 | 476 | DEBUG_PRINTLN(F("Writing final memory update 3/7 (function unknown)...")); 477 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 478 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 479 | 480 | DEBUG_PRINTLN(F("Writing final memory update 4/7 (function unknown)...")); 481 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 482 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 483 | 484 | DEBUG_PRINTLN(F("Writing final memory update 5/7 (function unknown)...")); 485 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 486 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 487 | 488 | DEBUG_PRINTLN(F("Waiting for FIFO count > 2...")); 489 | while ((fifoCount = getFIFOCount()) < 3); 490 | 491 | DEBUG_PRINT(F("Current FIFO count=")); 492 | DEBUG_PRINTLN(fifoCount); 493 | DEBUG_PRINTLN(F("Reading FIFO data...")); 494 | getFIFOBytes(fifoBuffer, fifoCount); 495 | 496 | DEBUG_PRINTLN(F("Reading interrupt status...")); 497 | uint8_t mpuIntStatus = getIntStatus(); 498 | 499 | DEBUG_PRINT(F("Current interrupt status=")); 500 | DEBUG_PRINTLNF(mpuIntStatus, HEX); 501 | 502 | DEBUG_PRINTLN(F("Reading final memory update 6/7 (function unknown)...")); 503 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 504 | readMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 505 | 506 | DEBUG_PRINTLN(F("Waiting for FIFO count > 2...")); 507 | while ((fifoCount = getFIFOCount()) < 3); 508 | 509 | DEBUG_PRINT(F("Current FIFO count=")); 510 | DEBUG_PRINTLN(fifoCount); 511 | 512 | DEBUG_PRINTLN(F("Reading FIFO data...")); 513 | getFIFOBytes(fifoBuffer, fifoCount); 514 | 515 | DEBUG_PRINTLN(F("Reading interrupt status...")); 516 | mpuIntStatus = getIntStatus(); 517 | 518 | DEBUG_PRINT(F("Current interrupt status=")); 519 | DEBUG_PRINTLNF(mpuIntStatus, HEX); 520 | 521 | DEBUG_PRINTLN(F("Writing final memory update 7/7 (function unknown)...")); 522 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 523 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 524 | 525 | DEBUG_PRINTLN(F("DMP is good to go! Finally.")); 526 | 527 | DEBUG_PRINTLN(F("Disabling DMP (you turn it on later)...")); 528 | setDMPEnabled(false); 529 | 530 | DEBUG_PRINTLN(F("Setting up internal 42-byte (default) DMP packet buffer...")); 531 | dmpPacketSize = 42; 532 | /*if ((dmpPacketBuffer = (uint8_t *)malloc(42)) == 0) { 533 | return 3; // TODO: proper error code for no memory 534 | }*/ 535 | 536 | DEBUG_PRINTLN(F("Resetting FIFO and clearing INT status one last time...")); 537 | resetFIFO(); 538 | getIntStatus(); 539 | } else { 540 | DEBUG_PRINTLN(F("ERROR! DMP configuration verification failed.")); 541 | return 2; // configuration block loading failed 542 | } 543 | } else { 544 | DEBUG_PRINTLN(F("ERROR! DMP code verification failed.")); 545 | return 1; // main binary block loading failed 546 | } 547 | return 0; // success 548 | } 549 | 550 | bool MPU6050::dmpPacketAvailable() { 551 | return getFIFOCount() >= dmpGetFIFOPacketSize(); 552 | } 553 | 554 | // uint8_t MPU6050::dmpSetFIFORate(uint8_t fifoRate); 555 | // uint8_t MPU6050::dmpGetFIFORate(); 556 | // uint8_t MPU6050::dmpGetSampleStepSizeMS(); 557 | // uint8_t MPU6050::dmpGetSampleFrequency(); 558 | // int32_t MPU6050::dmpDecodeTemperature(int8_t tempReg); 559 | 560 | //uint8_t MPU6050::dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority); 561 | //uint8_t MPU6050::dmpUnregisterFIFORateProcess(inv_obj_func func); 562 | //uint8_t MPU6050::dmpRunFIFORateProcesses(); 563 | 564 | // uint8_t MPU6050::dmpSendQuaternion(uint_fast16_t accuracy); 565 | // uint8_t MPU6050::dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy); 566 | // uint8_t MPU6050::dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy); 567 | // uint8_t MPU6050::dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy); 568 | // uint8_t MPU6050::dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy); 569 | // uint8_t MPU6050::dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy); 570 | // uint8_t MPU6050::dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy); 571 | // uint8_t MPU6050::dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy); 572 | // uint8_t MPU6050::dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy); 573 | // uint8_t MPU6050::dmpSendPacketNumber(uint_fast16_t accuracy); 574 | // uint8_t MPU6050::dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy); 575 | // uint8_t MPU6050::dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy); 576 | 577 | uint8_t MPU6050::dmpGetAccel(int32_t *data, const uint8_t* packet) { 578 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) 579 | if (packet == 0) packet = dmpPacketBuffer; 580 | data[0] = ((packet[28] << 24) + (packet[29] << 16) + (packet[30] << 8) + packet[31]); 581 | data[1] = ((packet[32] << 24) + (packet[33] << 16) + (packet[34] << 8) + packet[35]); 582 | data[2] = ((packet[36] << 24) + (packet[37] << 16) + (packet[38] << 8) + packet[39]); 583 | return 0; 584 | } 585 | uint8_t MPU6050::dmpGetAccel(int16_t *data, const uint8_t* packet) { 586 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) 587 | if (packet == 0) packet = dmpPacketBuffer; 588 | data[0] = (packet[28] << 8) + packet[29]; 589 | data[1] = (packet[32] << 8) + packet[33]; 590 | data[2] = (packet[36] << 8) + packet[37]; 591 | return 0; 592 | } 593 | uint8_t MPU6050::dmpGetAccel(VectorInt16 *v, const uint8_t* packet) { 594 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) 595 | if (packet == 0) packet = dmpPacketBuffer; 596 | v -> x = (packet[28] << 8) + packet[29]; 597 | v -> y = (packet[32] << 8) + packet[33]; 598 | v -> z = (packet[36] << 8) + packet[37]; 599 | return 0; 600 | } 601 | uint8_t MPU6050::dmpGetQuaternion(int32_t *data, const uint8_t* packet) { 602 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) 603 | if (packet == 0) packet = dmpPacketBuffer; 604 | data[0] = ((packet[0] << 24) + (packet[1] << 16) + (packet[2] << 8) + packet[3]); 605 | data[1] = ((packet[4] << 24) + (packet[5] << 16) + (packet[6] << 8) + packet[7]); 606 | data[2] = ((packet[8] << 24) + (packet[9] << 16) + (packet[10] << 8) + packet[11]); 607 | data[3] = ((packet[12] << 24) + (packet[13] << 16) + (packet[14] << 8) + packet[15]); 608 | return 0; 609 | } 610 | uint8_t MPU6050::dmpGetQuaternion(int16_t *data, const uint8_t* packet) { 611 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) 612 | if (packet == 0) packet = dmpPacketBuffer; 613 | data[0] = ((packet[0] << 8) + packet[1]); 614 | data[1] = ((packet[4] << 8) + packet[5]); 615 | data[2] = ((packet[8] << 8) + packet[9]); 616 | data[3] = ((packet[12] << 8) + packet[13]); 617 | return 0; 618 | } 619 | uint8_t MPU6050::dmpGetQuaternion(Quaternion *q, const uint8_t* packet) { 620 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) 621 | int16_t qI[4]; 622 | uint8_t status = dmpGetQuaternion(qI, packet); 623 | if (status == 0) { 624 | q -> w = (float)qI[0] / 16384.0f; 625 | q -> x = (float)qI[1] / 16384.0f; 626 | q -> y = (float)qI[2] / 16384.0f; 627 | q -> z = (float)qI[3] / 16384.0f; 628 | return 0; 629 | } 630 | return status; // int16 return value, indicates error if this line is reached 631 | } 632 | // uint8_t MPU6050::dmpGet6AxisQuaternion(long *data, const uint8_t* packet); 633 | // uint8_t MPU6050::dmpGetRelativeQuaternion(long *data, const uint8_t* packet); 634 | uint8_t MPU6050::dmpGetGyro(int32_t *data, const uint8_t* packet) { 635 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) 636 | if (packet == 0) packet = dmpPacketBuffer; 637 | data[0] = ((packet[16] << 24) + (packet[17] << 16) + (packet[18] << 8) + packet[19]); 638 | data[1] = ((packet[20] << 24) + (packet[21] << 16) + (packet[22] << 8) + packet[23]); 639 | data[2] = ((packet[24] << 24) + (packet[25] << 16) + (packet[26] << 8) + packet[27]); 640 | return 0; 641 | } 642 | uint8_t MPU6050::dmpGetGyro(int16_t *data, const uint8_t* packet) { 643 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) 644 | if (packet == 0) packet = dmpPacketBuffer; 645 | data[0] = (packet[16] << 8) + packet[17]; 646 | data[1] = (packet[20] << 8) + packet[21]; 647 | data[2] = (packet[24] << 8) + packet[25]; 648 | return 0; 649 | } 650 | // uint8_t MPU6050::dmpSetLinearAccelFilterCoefficient(float coef); 651 | // uint8_t MPU6050::dmpGetLinearAccel(long *data, const uint8_t* packet); 652 | uint8_t MPU6050::dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity) { 653 | // get rid of the gravity component (+1g = +8192 in standard DMP FIFO packet, sensitivity is 2g) 654 | v -> x = vRaw -> x - gravity -> x*8192; 655 | v -> y = vRaw -> y - gravity -> y*8192; 656 | v -> z = vRaw -> z - gravity -> z*8192; 657 | return 0; 658 | } 659 | // uint8_t MPU6050::dmpGetLinearAccelInWorld(long *data, const uint8_t* packet); 660 | uint8_t MPU6050::dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q) { 661 | // rotate measured 3D acceleration vector into original state 662 | // frame of reference based on orientation quaternion 663 | memcpy(v, vReal, sizeof(VectorInt16)); 664 | v -> rotate(q); 665 | return 0; 666 | } 667 | // uint8_t MPU6050::dmpGetGyroAndAccelSensor(long *data, const uint8_t* packet); 668 | // uint8_t MPU6050::dmpGetGyroSensor(long *data, const uint8_t* packet); 669 | // uint8_t MPU6050::dmpGetControlData(long *data, const uint8_t* packet); 670 | // uint8_t MPU6050::dmpGetTemperature(long *data, const uint8_t* packet); 671 | // uint8_t MPU6050::dmpGetGravity(long *data, const uint8_t* packet); 672 | uint8_t MPU6050::dmpGetGravity(VectorFloat *v, Quaternion *q) { 673 | v -> x = 2 * (q -> x*q -> z - q -> w*q -> y); 674 | v -> y = 2 * (q -> w*q -> x + q -> y*q -> z); 675 | v -> z = q -> w*q -> w - q -> x*q -> x - q -> y*q -> y + q -> z*q -> z; 676 | return 0; 677 | } 678 | // uint8_t MPU6050::dmpGetUnquantizedAccel(long *data, const uint8_t* packet); 679 | // uint8_t MPU6050::dmpGetQuantizedAccel(long *data, const uint8_t* packet); 680 | // uint8_t MPU6050::dmpGetExternalSensorData(long *data, int size, const uint8_t* packet); 681 | // uint8_t MPU6050::dmpGetEIS(long *data, const uint8_t* packet); 682 | 683 | uint8_t MPU6050::dmpGetEuler(float *data, Quaternion *q) { 684 | 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 685 | data[1] = -asin(2*q -> x*q -> z + 2*q -> w*q -> y); // theta 686 | 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 687 | return 0; 688 | } 689 | uint8_t MPU6050::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) { 690 | // yaw: (about Z axis) 691 | data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1); 692 | // pitch: (nose up/down, about Y axis) 693 | data[1] = atan(gravity -> x / sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z)); 694 | // roll: (tilt left/right, about X axis) 695 | data[2] = atan(gravity -> y / sqrt(gravity -> x*gravity -> x + gravity -> z*gravity -> z)); 696 | return 0; 697 | } 698 | 699 | // uint8_t MPU6050::dmpGetAccelFloat(float *data, const uint8_t* packet); 700 | // uint8_t MPU6050::dmpGetQuaternionFloat(float *data, const uint8_t* packet); 701 | 702 | uint8_t MPU6050::dmpProcessFIFOPacket(const unsigned char *dmpData) { 703 | /*for (uint8_t k = 0; k < dmpPacketSize; k++) { 704 | if (dmpData[k] < 0x10) Serial.print("0"); 705 | Serial.print(dmpData[k], HEX); 706 | Serial.print(" "); 707 | } 708 | Serial.print("\n");*/ 709 | //Serial.println((uint16_t)dmpPacketBuffer); 710 | return 0; 711 | } 712 | uint8_t MPU6050::dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed) { 713 | uint8_t status; 714 | uint8_t buf[dmpPacketSize]; 715 | for (uint8_t i = 0; i < numPackets; i++) { 716 | // read packet from FIFO 717 | getFIFOBytes(buf, dmpPacketSize); 718 | 719 | // process packet 720 | if ((status = dmpProcessFIFOPacket(buf)) > 0) return status; 721 | 722 | // increment external process count variable, if supplied 723 | if (processed != 0) *processed++; 724 | } 725 | return 0; 726 | } 727 | 728 | // uint8_t MPU6050::dmpSetFIFOProcessedCallback(void (*func) (void)); 729 | 730 | // uint8_t MPU6050::dmpInitFIFOParam(); 731 | // uint8_t MPU6050::dmpCloseFIFO(); 732 | // uint8_t MPU6050::dmpSetGyroDataSource(uint_fast8_t source); 733 | // uint8_t MPU6050::dmpDecodeQuantizedAccel(); 734 | // uint32_t MPU6050::dmpGetGyroSumOfSquare(); 735 | // uint32_t MPU6050::dmpGetAccelSumOfSquare(); 736 | // void MPU6050::dmpOverrideQuaternion(long *q); 737 | uint16_t MPU6050::dmpGetFIFOPacketSize() { 738 | return dmpPacketSize; 739 | } 740 | 741 | #endif /* _MPU6050_6AXIS_MOTIONAPPS20_H_ */ 742 | -------------------------------------------------------------------------------- /dev/MPU6050/MPU6050.h: -------------------------------------------------------------------------------- 1 | // I2Cdev library collection - MPU6050 I2C device class 2 | // Based on InvenSense MPU-6050 register map document rev. 2.0, 5/19/2011 (RM-MPU-6000A-00) 3 | // 10/3/2011 by Jeff Rowberg 4 | // Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib 5 | // 6 | // Changelog: 7 | // ... - ongoing debug release 8 | 9 | // NOTE: THIS IS ONLY A PARIAL RELEASE. THIS DEVICE CLASS IS CURRENTLY UNDERGOING ACTIVE 10 | // DEVELOPMENT AND IS STILL MISSING SOME IMPORTANT FEATURES. PLEASE KEEP THIS IN MIND IF 11 | // YOU DECIDE TO USE THIS PARTICULAR CODE FOR ANYTHING. 12 | 13 | /* ============================================ 14 | I2Cdev device library code is placed under the MIT license 15 | Copyright (c) 2012 Jeff Rowberg 16 | 17 | Permission is hereby granted, free of charge, to any person obtaining a copy 18 | of this software and associated documentation files (the "Software"), to deal 19 | in the Software without restriction, including without limitation the rights 20 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 21 | copies of the Software, and to permit persons to whom the Software is 22 | furnished to do so, subject to the following conditions: 23 | 24 | The above copyright notice and this permission notice shall be included in 25 | all copies or substantial portions of the Software. 26 | 27 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 28 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 29 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 30 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 31 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 32 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 33 | THE SOFTWARE. 34 | =============================================== 35 | */ 36 | 37 | #ifndef _MPU6050_H_ 38 | #define _MPU6050_H_ 39 | 40 | #include "I2Cdev.h" 41 | //#include 42 | 43 | 44 | 45 | #define MPU6050_ADDRESS_AD0_LOW 0x68 // address pin low (GND), default for InvenSense evaluation board 46 | #define MPU6050_ADDRESS_AD0_HIGH 0x69 // address pin high (VCC) 47 | #define MPU6050_DEFAULT_ADDRESS MPU6050_ADDRESS_AD0_LOW 48 | 49 | #define MPU6050_RA_XG_OFFS_TC 0x00 //[7] PWR_MODE, [6:1] XG_OFFS_TC, [0] OTP_BNK_VLD 50 | #define MPU6050_RA_YG_OFFS_TC 0x01 //[7] PWR_MODE, [6:1] YG_OFFS_TC, [0] OTP_BNK_VLD 51 | #define MPU6050_RA_ZG_OFFS_TC 0x02 //[7] PWR_MODE, [6:1] ZG_OFFS_TC, [0] OTP_BNK_VLD 52 | #define MPU6050_RA_X_FINE_GAIN 0x03 //[7:0] X_FINE_GAIN 53 | #define MPU6050_RA_Y_FINE_GAIN 0x04 //[7:0] Y_FINE_GAIN 54 | #define MPU6050_RA_Z_FINE_GAIN 0x05 //[7:0] Z_FINE_GAIN 55 | #define MPU6050_RA_XA_OFFS_H 0x06 //[15:0] XA_OFFS 56 | #define MPU6050_RA_XA_OFFS_L_TC 0x07 57 | #define MPU6050_RA_YA_OFFS_H 0x08 //[15:0] YA_OFFS 58 | #define MPU6050_RA_YA_OFFS_L_TC 0x09 59 | #define MPU6050_RA_ZA_OFFS_H 0x0A //[15:0] ZA_OFFS 60 | #define MPU6050_RA_ZA_OFFS_L_TC 0x0B 61 | #define MPU6050_RA_XG_OFFS_USRH 0x13 //[15:0] XG_OFFS_USR 62 | #define MPU6050_RA_XG_OFFS_USRL 0x14 63 | #define MPU6050_RA_YG_OFFS_USRH 0x15 //[15:0] YG_OFFS_USR 64 | #define MPU6050_RA_YG_OFFS_USRL 0x16 65 | #define MPU6050_RA_ZG_OFFS_USRH 0x17 //[15:0] ZG_OFFS_USR 66 | #define MPU6050_RA_ZG_OFFS_USRL 0x18 67 | #define MPU6050_RA_SMPLRT_DIV 0x19 68 | #define MPU6050_RA_CONFIG 0x1A 69 | #define MPU6050_RA_GYRO_CONFIG 0x1B 70 | #define MPU6050_RA_ACCEL_CONFIG 0x1C 71 | #define MPU6050_RA_FF_THR 0x1D 72 | #define MPU6050_RA_FF_DUR 0x1E 73 | #define MPU6050_RA_MOT_THR 0x1F 74 | #define MPU6050_RA_MOT_DUR 0x20 75 | #define MPU6050_RA_ZRMOT_THR 0x21 76 | #define MPU6050_RA_ZRMOT_DUR 0x22 77 | #define MPU6050_RA_FIFO_EN 0x23 78 | #define MPU6050_RA_I2C_MST_CTRL 0x24 79 | #define MPU6050_RA_I2C_SLV0_ADDR 0x25 80 | #define MPU6050_RA_I2C_SLV0_REG 0x26 81 | #define MPU6050_RA_I2C_SLV0_CTRL 0x27 82 | #define MPU6050_RA_I2C_SLV1_ADDR 0x28 83 | #define MPU6050_RA_I2C_SLV1_REG 0x29 84 | #define MPU6050_RA_I2C_SLV1_CTRL 0x2A 85 | #define MPU6050_RA_I2C_SLV2_ADDR 0x2B 86 | #define MPU6050_RA_I2C_SLV2_REG 0x2C 87 | #define MPU6050_RA_I2C_SLV2_CTRL 0x2D 88 | #define MPU6050_RA_I2C_SLV3_ADDR 0x2E 89 | #define MPU6050_RA_I2C_SLV3_REG 0x2F 90 | #define MPU6050_RA_I2C_SLV3_CTRL 0x30 91 | #define MPU6050_RA_I2C_SLV4_ADDR 0x31 92 | #define MPU6050_RA_I2C_SLV4_REG 0x32 93 | #define MPU6050_RA_I2C_SLV4_DO 0x33 94 | #define MPU6050_RA_I2C_SLV4_CTRL 0x34 95 | #define MPU6050_RA_I2C_SLV4_DI 0x35 96 | #define MPU6050_RA_I2C_MST_STATUS 0x36 97 | #define MPU6050_RA_INT_PIN_CFG 0x37 98 | #define MPU6050_RA_INT_ENABLE 0x38 99 | #define MPU6050_RA_DMP_INT_STATUS 0x39 100 | #define MPU6050_RA_INT_STATUS 0x3A 101 | #define MPU6050_RA_ACCEL_XOUT_H 0x3B 102 | #define MPU6050_RA_ACCEL_XOUT_L 0x3C 103 | #define MPU6050_RA_ACCEL_YOUT_H 0x3D 104 | #define MPU6050_RA_ACCEL_YOUT_L 0x3E 105 | #define MPU6050_RA_ACCEL_ZOUT_H 0x3F 106 | #define MPU6050_RA_ACCEL_ZOUT_L 0x40 107 | #define MPU6050_RA_TEMP_OUT_H 0x41 108 | #define MPU6050_RA_TEMP_OUT_L 0x42 109 | #define MPU6050_RA_GYRO_XOUT_H 0x43 110 | #define MPU6050_RA_GYRO_XOUT_L 0x44 111 | #define MPU6050_RA_GYRO_YOUT_H 0x45 112 | #define MPU6050_RA_GYRO_YOUT_L 0x46 113 | #define MPU6050_RA_GYRO_ZOUT_H 0x47 114 | #define MPU6050_RA_GYRO_ZOUT_L 0x48 115 | #define MPU6050_RA_EXT_SENS_DATA_00 0x49 116 | #define MPU6050_RA_EXT_SENS_DATA_01 0x4A 117 | #define MPU6050_RA_EXT_SENS_DATA_02 0x4B 118 | #define MPU6050_RA_EXT_SENS_DATA_03 0x4C 119 | #define MPU6050_RA_EXT_SENS_DATA_04 0x4D 120 | #define MPU6050_RA_EXT_SENS_DATA_05 0x4E 121 | #define MPU6050_RA_EXT_SENS_DATA_06 0x4F 122 | #define MPU6050_RA_EXT_SENS_DATA_07 0x50 123 | #define MPU6050_RA_EXT_SENS_DATA_08 0x51 124 | #define MPU6050_RA_EXT_SENS_DATA_09 0x52 125 | #define MPU6050_RA_EXT_SENS_DATA_10 0x53 126 | #define MPU6050_RA_EXT_SENS_DATA_11 0x54 127 | #define MPU6050_RA_EXT_SENS_DATA_12 0x55 128 | #define MPU6050_RA_EXT_SENS_DATA_13 0x56 129 | #define MPU6050_RA_EXT_SENS_DATA_14 0x57 130 | #define MPU6050_RA_EXT_SENS_DATA_15 0x58 131 | #define MPU6050_RA_EXT_SENS_DATA_16 0x59 132 | #define MPU6050_RA_EXT_SENS_DATA_17 0x5A 133 | #define MPU6050_RA_EXT_SENS_DATA_18 0x5B 134 | #define MPU6050_RA_EXT_SENS_DATA_19 0x5C 135 | #define MPU6050_RA_EXT_SENS_DATA_20 0x5D 136 | #define MPU6050_RA_EXT_SENS_DATA_21 0x5E 137 | #define MPU6050_RA_EXT_SENS_DATA_22 0x5F 138 | #define MPU6050_RA_EXT_SENS_DATA_23 0x60 139 | #define MPU6050_RA_MOT_DETECT_STATUS 0x61 140 | #define MPU6050_RA_I2C_SLV0_DO 0x63 141 | #define MPU6050_RA_I2C_SLV1_DO 0x64 142 | #define MPU6050_RA_I2C_SLV2_DO 0x65 143 | #define MPU6050_RA_I2C_SLV3_DO 0x66 144 | #define MPU6050_RA_I2C_MST_DELAY_CTRL 0x67 145 | #define MPU6050_RA_SIGNAL_PATH_RESET 0x68 146 | #define MPU6050_RA_MOT_DETECT_CTRL 0x69 147 | #define MPU6050_RA_USER_CTRL 0x6A 148 | #define MPU6050_RA_PWR_MGMT_1 0x6B 149 | #define MPU6050_RA_PWR_MGMT_2 0x6C 150 | #define MPU6050_RA_BANK_SEL 0x6D 151 | #define MPU6050_RA_MEM_START_ADDR 0x6E 152 | #define MPU6050_RA_MEM_R_W 0x6F 153 | #define MPU6050_RA_DMP_CFG_1 0x70 154 | #define MPU6050_RA_DMP_CFG_2 0x71 155 | #define MPU6050_RA_FIFO_COUNTH 0x72 156 | #define MPU6050_RA_FIFO_COUNTL 0x73 157 | #define MPU6050_RA_FIFO_R_W 0x74 158 | #define MPU6050_RA_WHO_AM_I 0x75 159 | 160 | #define MPU6050_TC_PWR_MODE_BIT 7 161 | #define MPU6050_TC_OFFSET_BIT 6 162 | #define MPU6050_TC_OFFSET_LENGTH 6 163 | #define MPU6050_TC_OTP_BNK_VLD_BIT 0 164 | 165 | #define MPU6050_VDDIO_LEVEL_VLOGIC 0 166 | #define MPU6050_VDDIO_LEVEL_VDD 1 167 | 168 | #define MPU6050_CFG_EXT_SYNC_SET_BIT 5 169 | #define MPU6050_CFG_EXT_SYNC_SET_LENGTH 3 170 | #define MPU6050_CFG_DLPF_CFG_BIT 2 171 | #define MPU6050_CFG_DLPF_CFG_LENGTH 3 172 | 173 | #define MPU6050_EXT_SYNC_DISABLED 0x0 174 | #define MPU6050_EXT_SYNC_TEMP_OUT_L 0x1 175 | #define MPU6050_EXT_SYNC_GYRO_XOUT_L 0x2 176 | #define MPU6050_EXT_SYNC_GYRO_YOUT_L 0x3 177 | #define MPU6050_EXT_SYNC_GYRO_ZOUT_L 0x4 178 | #define MPU6050_EXT_SYNC_ACCEL_XOUT_L 0x5 179 | #define MPU6050_EXT_SYNC_ACCEL_YOUT_L 0x6 180 | #define MPU6050_EXT_SYNC_ACCEL_ZOUT_L 0x7 181 | 182 | #define MPU6050_DLPF_BW_256 0x00 183 | #define MPU6050_DLPF_BW_188 0x01 184 | #define MPU6050_DLPF_BW_98 0x02 185 | #define MPU6050_DLPF_BW_42 0x03 186 | #define MPU6050_DLPF_BW_20 0x04 187 | #define MPU6050_DLPF_BW_10 0x05 188 | #define MPU6050_DLPF_BW_5 0x06 189 | 190 | #define MPU6050_GCONFIG_FS_SEL_BIT 4 191 | #define MPU6050_GCONFIG_FS_SEL_LENGTH 2 192 | 193 | #define MPU6050_GYRO_FS_250 0x00 194 | #define MPU6050_GYRO_FS_500 0x01 195 | #define MPU6050_GYRO_FS_1000 0x02 196 | #define MPU6050_GYRO_FS_2000 0x03 197 | 198 | #define MPU6050_ACONFIG_XA_ST_BIT 7 199 | #define MPU6050_ACONFIG_YA_ST_BIT 6 200 | #define MPU6050_ACONFIG_ZA_ST_BIT 5 201 | #define MPU6050_ACONFIG_AFS_SEL_BIT 4 202 | #define MPU6050_ACONFIG_AFS_SEL_LENGTH 2 203 | #define MPU6050_ACONFIG_ACCEL_HPF_BIT 2 204 | #define MPU6050_ACONFIG_ACCEL_HPF_LENGTH 3 205 | 206 | #define MPU6050_ACCEL_FS_2 0x00 207 | #define MPU6050_ACCEL_FS_4 0x01 208 | #define MPU6050_ACCEL_FS_8 0x02 209 | #define MPU6050_ACCEL_FS_16 0x03 210 | 211 | #define MPU6050_DHPF_RESET 0x00 212 | #define MPU6050_DHPF_5 0x01 213 | #define MPU6050_DHPF_2P5 0x02 214 | #define MPU6050_DHPF_1P25 0x03 215 | #define MPU6050_DHPF_0P63 0x04 216 | #define MPU6050_DHPF_HOLD 0x07 217 | 218 | #define MPU6050_TEMP_FIFO_EN_BIT 7 219 | #define MPU6050_XG_FIFO_EN_BIT 6 220 | #define MPU6050_YG_FIFO_EN_BIT 5 221 | #define MPU6050_ZG_FIFO_EN_BIT 4 222 | #define MPU6050_ACCEL_FIFO_EN_BIT 3 223 | #define MPU6050_SLV2_FIFO_EN_BIT 2 224 | #define MPU6050_SLV1_FIFO_EN_BIT 1 225 | #define MPU6050_SLV0_FIFO_EN_BIT 0 226 | 227 | #define MPU6050_MULT_MST_EN_BIT 7 228 | #define MPU6050_WAIT_FOR_ES_BIT 6 229 | #define MPU6050_SLV_3_FIFO_EN_BIT 5 230 | #define MPU6050_I2C_MST_P_NSR_BIT 4 231 | #define MPU6050_I2C_MST_CLK_BIT 3 232 | #define MPU6050_I2C_MST_CLK_LENGTH 4 233 | 234 | #define MPU6050_CLOCK_DIV_348 0x0 235 | #define MPU6050_CLOCK_DIV_333 0x1 236 | #define MPU6050_CLOCK_DIV_320 0x2 237 | #define MPU6050_CLOCK_DIV_308 0x3 238 | #define MPU6050_CLOCK_DIV_296 0x4 239 | #define MPU6050_CLOCK_DIV_286 0x5 240 | #define MPU6050_CLOCK_DIV_276 0x6 241 | #define MPU6050_CLOCK_DIV_267 0x7 242 | #define MPU6050_CLOCK_DIV_258 0x8 243 | #define MPU6050_CLOCK_DIV_500 0x9 244 | #define MPU6050_CLOCK_DIV_471 0xA 245 | #define MPU6050_CLOCK_DIV_444 0xB 246 | #define MPU6050_CLOCK_DIV_421 0xC 247 | #define MPU6050_CLOCK_DIV_400 0xD 248 | #define MPU6050_CLOCK_DIV_381 0xE 249 | #define MPU6050_CLOCK_DIV_364 0xF 250 | 251 | #define MPU6050_I2C_SLV_RW_BIT 7 252 | #define MPU6050_I2C_SLV_ADDR_BIT 6 253 | #define MPU6050_I2C_SLV_ADDR_LENGTH 7 254 | #define MPU6050_I2C_SLV_EN_BIT 7 255 | #define MPU6050_I2C_SLV_BYTE_SW_BIT 6 256 | #define MPU6050_I2C_SLV_REG_DIS_BIT 5 257 | #define MPU6050_I2C_SLV_GRP_BIT 4 258 | #define MPU6050_I2C_SLV_LEN_BIT 3 259 | #define MPU6050_I2C_SLV_LEN_LENGTH 4 260 | 261 | #define MPU6050_I2C_SLV4_RW_BIT 7 262 | #define MPU6050_I2C_SLV4_ADDR_BIT 6 263 | #define MPU6050_I2C_SLV4_ADDR_LENGTH 7 264 | #define MPU6050_I2C_SLV4_EN_BIT 7 265 | #define MPU6050_I2C_SLV4_INT_EN_BIT 6 266 | #define MPU6050_I2C_SLV4_REG_DIS_BIT 5 267 | #define MPU6050_I2C_SLV4_MST_DLY_BIT 4 268 | #define MPU6050_I2C_SLV4_MST_DLY_LENGTH 5 269 | 270 | #define MPU6050_MST_PASS_THROUGH_BIT 7 271 | #define MPU6050_MST_I2C_SLV4_DONE_BIT 6 272 | #define MPU6050_MST_I2C_LOST_ARB_BIT 5 273 | #define MPU6050_MST_I2C_SLV4_NACK_BIT 4 274 | #define MPU6050_MST_I2C_SLV3_NACK_BIT 3 275 | #define MPU6050_MST_I2C_SLV2_NACK_BIT 2 276 | #define MPU6050_MST_I2C_SLV1_NACK_BIT 1 277 | #define MPU6050_MST_I2C_SLV0_NACK_BIT 0 278 | 279 | #define MPU6050_INTCFG_INT_LEVEL_BIT 7 280 | #define MPU6050_INTCFG_INT_OPEN_BIT 6 281 | #define MPU6050_INTCFG_LATCH_INT_EN_BIT 5 282 | #define MPU6050_INTCFG_INT_RD_CLEAR_BIT 4 283 | #define MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT 3 284 | #define MPU6050_INTCFG_FSYNC_INT_EN_BIT 2 285 | #define MPU6050_INTCFG_I2C_BYPASS_EN_BIT 1 286 | #define MPU6050_INTCFG_CLKOUT_EN_BIT 0 287 | 288 | #define MPU6050_INTMODE_ACTIVEHIGH 0x00 289 | #define MPU6050_INTMODE_ACTIVELOW 0x01 290 | 291 | #define MPU6050_INTDRV_PUSHPULL 0x00 292 | #define MPU6050_INTDRV_OPENDRAIN 0x01 293 | 294 | #define MPU6050_INTLATCH_50USPULSE 0x00 295 | #define MPU6050_INTLATCH_WAITCLEAR 0x01 296 | 297 | #define MPU6050_INTCLEAR_STATUSREAD 0x00 298 | #define MPU6050_INTCLEAR_ANYREAD 0x01 299 | 300 | #define MPU6050_INTERRUPT_FF_BIT 7 301 | #define MPU6050_INTERRUPT_MOT_BIT 6 302 | #define MPU6050_INTERRUPT_ZMOT_BIT 5 303 | #define MPU6050_INTERRUPT_FIFO_OFLOW_BIT 4 304 | #define MPU6050_INTERRUPT_I2C_MST_INT_BIT 3 305 | #define MPU6050_INTERRUPT_PLL_RDY_INT_BIT 2 306 | #define MPU6050_INTERRUPT_DMP_INT_BIT 1 307 | #define MPU6050_INTERRUPT_DATA_RDY_BIT 0 308 | 309 | // TODO: figure out what these actually do 310 | // UMPL source code is not very obivous 311 | #define MPU6050_DMPINT_5_BIT 5 312 | #define MPU6050_DMPINT_4_BIT 4 313 | #define MPU6050_DMPINT_3_BIT 3 314 | #define MPU6050_DMPINT_2_BIT 2 315 | #define MPU6050_DMPINT_1_BIT 1 316 | #define MPU6050_DMPINT_0_BIT 0 317 | 318 | #define MPU6050_MOTION_MOT_XNEG_BIT 7 319 | #define MPU6050_MOTION_MOT_XPOS_BIT 6 320 | #define MPU6050_MOTION_MOT_YNEG_BIT 5 321 | #define MPU6050_MOTION_MOT_YPOS_BIT 4 322 | #define MPU6050_MOTION_MOT_ZNEG_BIT 3 323 | #define MPU6050_MOTION_MOT_ZPOS_BIT 2 324 | #define MPU6050_MOTION_MOT_ZRMOT_BIT 0 325 | 326 | #define MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT 7 327 | #define MPU6050_DELAYCTRL_I2C_SLV4_DLY_EN_BIT 4 328 | #define MPU6050_DELAYCTRL_I2C_SLV3_DLY_EN_BIT 3 329 | #define MPU6050_DELAYCTRL_I2C_SLV2_DLY_EN_BIT 2 330 | #define MPU6050_DELAYCTRL_I2C_SLV1_DLY_EN_BIT 1 331 | #define MPU6050_DELAYCTRL_I2C_SLV0_DLY_EN_BIT 0 332 | 333 | #define MPU6050_PATHRESET_GYRO_RESET_BIT 2 334 | #define MPU6050_PATHRESET_ACCEL_RESET_BIT 1 335 | #define MPU6050_PATHRESET_TEMP_RESET_BIT 0 336 | 337 | #define MPU6050_DETECT_ACCEL_ON_DELAY_BIT 5 338 | #define MPU6050_DETECT_ACCEL_ON_DELAY_LENGTH 2 339 | #define MPU6050_DETECT_FF_COUNT_BIT 3 340 | #define MPU6050_DETECT_FF_COUNT_LENGTH 2 341 | #define MPU6050_DETECT_MOT_COUNT_BIT 1 342 | #define MPU6050_DETECT_MOT_COUNT_LENGTH 2 343 | 344 | #define MPU6050_DETECT_DECREMENT_RESET 0x0 345 | #define MPU6050_DETECT_DECREMENT_1 0x1 346 | #define MPU6050_DETECT_DECREMENT_2 0x2 347 | #define MPU6050_DETECT_DECREMENT_4 0x3 348 | 349 | #define MPU6050_USERCTRL_DMP_EN_BIT 7 350 | #define MPU6050_USERCTRL_FIFO_EN_BIT 6 351 | #define MPU6050_USERCTRL_I2C_MST_EN_BIT 5 352 | #define MPU6050_USERCTRL_I2C_IF_DIS_BIT 4 353 | #define MPU6050_USERCTRL_DMP_RESET_BIT 3 354 | #define MPU6050_USERCTRL_FIFO_RESET_BIT 2 355 | #define MPU6050_USERCTRL_I2C_MST_RESET_BIT 1 356 | #define MPU6050_USERCTRL_SIG_COND_RESET_BIT 0 357 | 358 | #define MPU6050_PWR1_DEVICE_RESET_BIT 7 359 | #define MPU6050_PWR1_SLEEP_BIT 6 360 | #define MPU6050_PWR1_CYCLE_BIT 5 361 | #define MPU6050_PWR1_TEMP_DIS_BIT 3 362 | #define MPU6050_PWR1_CLKSEL_BIT 2 363 | #define MPU6050_PWR1_CLKSEL_LENGTH 3 364 | 365 | #define MPU6050_CLOCK_INTERNAL 0x00 366 | #define MPU6050_CLOCK_PLL_XGYRO 0x01 367 | #define MPU6050_CLOCK_PLL_YGYRO 0x02 368 | #define MPU6050_CLOCK_PLL_ZGYRO 0x03 369 | #define MPU6050_CLOCK_PLL_EXT32K 0x04 370 | #define MPU6050_CLOCK_PLL_EXT19M 0x05 371 | #define MPU6050_CLOCK_KEEP_RESET 0x07 372 | 373 | #define MPU6050_PWR2_LP_WAKE_CTRL_BIT 7 374 | #define MPU6050_PWR2_LP_WAKE_CTRL_LENGTH 2 375 | #define MPU6050_PWR2_STBY_XA_BIT 5 376 | #define MPU6050_PWR2_STBY_YA_BIT 4 377 | #define MPU6050_PWR2_STBY_ZA_BIT 3 378 | #define MPU6050_PWR2_STBY_XG_BIT 2 379 | #define MPU6050_PWR2_STBY_YG_BIT 1 380 | #define MPU6050_PWR2_STBY_ZG_BIT 0 381 | 382 | #define MPU6050_WAKE_FREQ_1P25 0x0 383 | #define MPU6050_WAKE_FREQ_2P5 0x1 384 | #define MPU6050_WAKE_FREQ_5 0x2 385 | #define MPU6050_WAKE_FREQ_10 0x3 386 | 387 | #define MPU6050_BANKSEL_PRFTCH_EN_BIT 6 388 | #define MPU6050_BANKSEL_CFG_USER_BANK_BIT 5 389 | #define MPU6050_BANKSEL_MEM_SEL_BIT 4 390 | #define MPU6050_BANKSEL_MEM_SEL_LENGTH 5 391 | 392 | #define MPU6050_WHO_AM_I_BIT 6 393 | #define MPU6050_WHO_AM_I_LENGTH 6 394 | 395 | #define MPU6050_DMP_MEMORY_BANKS 8 396 | #define MPU6050_DMP_MEMORY_BANK_SIZE 256 397 | #define MPU6050_DMP_MEMORY_CHUNK_SIZE 16 398 | 399 | // note: DMP code memory blocks defined at end of header file 400 | 401 | class MPU6050 { 402 | public: 403 | MPU6050(); 404 | MPU6050(uint8_t address); 405 | 406 | void initialize(); 407 | bool testConnection(); 408 | 409 | // AUX_VDDIO register 410 | uint8_t getAuxVDDIOLevel(); 411 | void setAuxVDDIOLevel(uint8_t level); 412 | 413 | // SMPLRT_DIV register 414 | uint8_t getRate(); 415 | void setRate(uint8_t rate); 416 | 417 | // CONFIG register 418 | uint8_t getExternalFrameSync(); 419 | void setExternalFrameSync(uint8_t sync); 420 | uint8_t getDLPFMode(); 421 | void setDLPFMode(uint8_t bandwidth); 422 | 423 | // GYRO_CONFIG register 424 | uint8_t getFullScaleGyroRange(); 425 | void setFullScaleGyroRange(uint8_t range); 426 | 427 | // ACCEL_CONFIG register 428 | bool getAccelXSelfTest(); 429 | void setAccelXSelfTest(bool enabled); 430 | bool getAccelYSelfTest(); 431 | void setAccelYSelfTest(bool enabled); 432 | bool getAccelZSelfTest(); 433 | void setAccelZSelfTest(bool enabled); 434 | uint8_t getFullScaleAccelRange(); 435 | void setFullScaleAccelRange(uint8_t range); 436 | uint8_t getDHPFMode(); 437 | void setDHPFMode(uint8_t mode); 438 | 439 | // FF_THR register 440 | uint8_t getFreefallDetectionThreshold(); 441 | void setFreefallDetectionThreshold(uint8_t threshold); 442 | 443 | // FF_DUR register 444 | uint8_t getFreefallDetectionDuration(); 445 | void setFreefallDetectionDuration(uint8_t duration); 446 | 447 | // MOT_THR register 448 | uint8_t getMotionDetectionThreshold(); 449 | void setMotionDetectionThreshold(uint8_t threshold); 450 | 451 | // MOT_DUR register 452 | uint8_t getMotionDetectionDuration(); 453 | void setMotionDetectionDuration(uint8_t duration); 454 | 455 | // ZRMOT_THR register 456 | uint8_t getZeroMotionDetectionThreshold(); 457 | void setZeroMotionDetectionThreshold(uint8_t threshold); 458 | 459 | // ZRMOT_DUR register 460 | uint8_t getZeroMotionDetectionDuration(); 461 | void setZeroMotionDetectionDuration(uint8_t duration); 462 | 463 | // FIFO_EN register 464 | bool getTempFIFOEnabled(); 465 | void setTempFIFOEnabled(bool enabled); 466 | bool getXGyroFIFOEnabled(); 467 | void setXGyroFIFOEnabled(bool enabled); 468 | bool getYGyroFIFOEnabled(); 469 | void setYGyroFIFOEnabled(bool enabled); 470 | bool getZGyroFIFOEnabled(); 471 | void setZGyroFIFOEnabled(bool enabled); 472 | bool getAccelFIFOEnabled(); 473 | void setAccelFIFOEnabled(bool enabled); 474 | bool getSlave2FIFOEnabled(); 475 | void setSlave2FIFOEnabled(bool enabled); 476 | bool getSlave1FIFOEnabled(); 477 | void setSlave1FIFOEnabled(bool enabled); 478 | bool getSlave0FIFOEnabled(); 479 | void setSlave0FIFOEnabled(bool enabled); 480 | 481 | // I2C_MST_CTRL register 482 | bool getMultiMasterEnabled(); 483 | void setMultiMasterEnabled(bool enabled); 484 | bool getWaitForExternalSensorEnabled(); 485 | void setWaitForExternalSensorEnabled(bool enabled); 486 | bool getSlave3FIFOEnabled(); 487 | void setSlave3FIFOEnabled(bool enabled); 488 | bool getSlaveReadWriteTransitionEnabled(); 489 | void setSlaveReadWriteTransitionEnabled(bool enabled); 490 | uint8_t getMasterClockSpeed(); 491 | void setMasterClockSpeed(uint8_t speed); 492 | 493 | // I2C_SLV* registers (Slave 0-3) 494 | uint8_t getSlaveAddress(uint8_t num); 495 | void setSlaveAddress(uint8_t num, uint8_t address); 496 | uint8_t getSlaveRegister(uint8_t num); 497 | void setSlaveRegister(uint8_t num, uint8_t reg); 498 | bool getSlaveEnabled(uint8_t num); 499 | void setSlaveEnabled(uint8_t num, bool enabled); 500 | bool getSlaveWordByteSwap(uint8_t num); 501 | void setSlaveWordByteSwap(uint8_t num, bool enabled); 502 | bool getSlaveWriteMode(uint8_t num); 503 | void setSlaveWriteMode(uint8_t num, bool mode); 504 | bool getSlaveWordGroupOffset(uint8_t num); 505 | void setSlaveWordGroupOffset(uint8_t num, bool enabled); 506 | uint8_t getSlaveDataLength(uint8_t num); 507 | void setSlaveDataLength(uint8_t num, uint8_t length); 508 | 509 | // I2C_SLV* registers (Slave 4) 510 | uint8_t getSlave4Address(); 511 | void setSlave4Address(uint8_t address); 512 | uint8_t getSlave4Register(); 513 | void setSlave4Register(uint8_t reg); 514 | void setSlave4OutputByte(uint8_t data); 515 | bool getSlave4Enabled(); 516 | void setSlave4Enabled(bool enabled); 517 | bool getSlave4InterruptEnabled(); 518 | void setSlave4InterruptEnabled(bool enabled); 519 | bool getSlave4WriteMode(); 520 | void setSlave4WriteMode(bool mode); 521 | uint8_t getSlave4MasterDelay(); 522 | void setSlave4MasterDelay(uint8_t delay); 523 | uint8_t getSlate4InputByte(); 524 | 525 | // I2C_MST_STATUS register 526 | bool getPassthroughStatus(); 527 | bool getSlave4IsDone(); 528 | bool getLostArbitration(); 529 | bool getSlave4Nack(); 530 | bool getSlave3Nack(); 531 | bool getSlave2Nack(); 532 | bool getSlave1Nack(); 533 | bool getSlave0Nack(); 534 | 535 | // INT_PIN_CFG register 536 | bool getInterruptMode(); 537 | void setInterruptMode(bool mode); 538 | bool getInterruptDrive(); 539 | void setInterruptDrive(bool drive); 540 | bool getInterruptLatch(); 541 | void setInterruptLatch(bool latch); 542 | bool getInterruptLatchClear(); 543 | void setInterruptLatchClear(bool clear); 544 | bool getFSyncInterruptLevel(); 545 | void setFSyncInterruptLevel(bool level); 546 | bool getFSyncInterruptEnabled(); 547 | void setFSyncInterruptEnabled(bool enabled); 548 | bool getI2CBypassEnabled(); 549 | void setI2CBypassEnabled(bool enabled); 550 | bool getClockOutputEnabled(); 551 | void setClockOutputEnabled(bool enabled); 552 | 553 | // INT_ENABLE register 554 | uint8_t getIntEnabled(); 555 | void setIntEnabled(uint8_t enabled); 556 | bool getIntFreefallEnabled(); 557 | void setIntFreefallEnabled(bool enabled); 558 | bool getIntMotionEnabled(); 559 | void setIntMotionEnabled(bool enabled); 560 | bool getIntZeroMotionEnabled(); 561 | void setIntZeroMotionEnabled(bool enabled); 562 | bool getIntFIFOBufferOverflowEnabled(); 563 | void setIntFIFOBufferOverflowEnabled(bool enabled); 564 | bool getIntI2CMasterEnabled(); 565 | void setIntI2CMasterEnabled(bool enabled); 566 | bool getIntDataReadyEnabled(); 567 | void setIntDataReadyEnabled(bool enabled); 568 | 569 | // INT_STATUS register 570 | uint8_t getIntStatus(); 571 | bool getIntFreefallStatus(); 572 | bool getIntMotionStatus(); 573 | bool getIntZeroMotionStatus(); 574 | bool getIntFIFOBufferOverflowStatus(); 575 | bool getIntI2CMasterStatus(); 576 | bool getIntDataReadyStatus(); 577 | 578 | // ACCEL_*OUT_* registers 579 | void getMotion9(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz, int16_t* mx, int16_t* my, int16_t* mz); 580 | void getMotion6(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz); 581 | void getAcceleration(int16_t* x, int16_t* y, int16_t* z); 582 | int16_t getAccelerationX(); 583 | int16_t getAccelerationY(); 584 | int16_t getAccelerationZ(); 585 | 586 | // TEMP_OUT_* registers 587 | int16_t getTemperature(); 588 | 589 | // GYRO_*OUT_* registers 590 | void getRotation(int16_t* x, int16_t* y, int16_t* z); 591 | int16_t getRotationX(); 592 | int16_t getRotationY(); 593 | int16_t getRotationZ(); 594 | 595 | // EXT_SENS_DATA_* registers 596 | uint8_t getExternalSensorByte(int position); 597 | uint16_t getExternalSensorWord(int position); 598 | uint32_t getExternalSensorDWord(int position); 599 | 600 | // MOT_DETECT_STATUS register 601 | bool getXNegMotionDetected(); 602 | bool getXPosMotionDetected(); 603 | bool getYNegMotionDetected(); 604 | bool getYPosMotionDetected(); 605 | bool getZNegMotionDetected(); 606 | bool getZPosMotionDetected(); 607 | bool getZeroMotionDetected(); 608 | 609 | // I2C_SLV*_DO register 610 | void setSlaveOutputByte(uint8_t num, uint8_t data); 611 | 612 | // I2C_MST_DELAY_CTRL register 613 | bool getExternalShadowDelayEnabled(); 614 | void setExternalShadowDelayEnabled(bool enabled); 615 | bool getSlaveDelayEnabled(uint8_t num); 616 | void setSlaveDelayEnabled(uint8_t num, bool enabled); 617 | 618 | // SIGNAL_PATH_RESET register 619 | void resetGyroscopePath(); 620 | void resetAccelerometerPath(); 621 | void resetTemperaturePath(); 622 | 623 | // MOT_DETECT_CTRL register 624 | uint8_t getAccelerometerPowerOnDelay(); 625 | void setAccelerometerPowerOnDelay(uint8_t delay); 626 | uint8_t getFreefallDetectionCounterDecrement(); 627 | void setFreefallDetectionCounterDecrement(uint8_t decrement); 628 | uint8_t getMotionDetectionCounterDecrement(); 629 | void setMotionDetectionCounterDecrement(uint8_t decrement); 630 | 631 | // USER_CTRL register 632 | bool getFIFOEnabled(); 633 | void setFIFOEnabled(bool enabled); 634 | bool getI2CMasterModeEnabled(); 635 | void setI2CMasterModeEnabled(bool enabled); 636 | void switchSPIEnabled(bool enabled); 637 | void resetFIFO(); 638 | void resetI2CMaster(); 639 | void resetSensors(); 640 | 641 | // PWR_MGMT_1 register 642 | void reset(); 643 | bool getSleepEnabled(); 644 | void setSleepEnabled(bool enabled); 645 | bool getWakeCycleEnabled(); 646 | void setWakeCycleEnabled(bool enabled); 647 | bool getTempSensorEnabled(); 648 | void setTempSensorEnabled(bool enabled); 649 | uint8_t getClockSource(); 650 | void setClockSource(uint8_t source); 651 | 652 | // PWR_MGMT_2 register 653 | uint8_t getWakeFrequency(); 654 | void setWakeFrequency(uint8_t frequency); 655 | bool getStandbyXAccelEnabled(); 656 | void setStandbyXAccelEnabled(bool enabled); 657 | bool getStandbyYAccelEnabled(); 658 | void setStandbyYAccelEnabled(bool enabled); 659 | bool getStandbyZAccelEnabled(); 660 | void setStandbyZAccelEnabled(bool enabled); 661 | bool getStandbyXGyroEnabled(); 662 | void setStandbyXGyroEnabled(bool enabled); 663 | bool getStandbyYGyroEnabled(); 664 | void setStandbyYGyroEnabled(bool enabled); 665 | bool getStandbyZGyroEnabled(); 666 | void setStandbyZGyroEnabled(bool enabled); 667 | 668 | // FIFO_COUNT_* registers 669 | uint16_t getFIFOCount(); 670 | 671 | // FIFO_R_W register 672 | uint8_t getFIFOByte(); 673 | void setFIFOByte(uint8_t data); 674 | void getFIFOBytes(uint8_t *data, uint8_t length); 675 | 676 | // WHO_AM_I register 677 | uint8_t getDeviceID(); 678 | void setDeviceID(uint8_t id); 679 | 680 | // ======== UNDOCUMENTED/DMP REGISTERS/METHODS ======== 681 | 682 | // XG_OFFS_TC register 683 | uint8_t getOTPBankValid(); 684 | void setOTPBankValid(bool enabled); 685 | int8_t getXGyroOffsetTC(); 686 | void setXGyroOffsetTC(int8_t offset); 687 | 688 | // YG_OFFS_TC register 689 | int8_t getYGyroOffsetTC(); 690 | void setYGyroOffsetTC(int8_t offset); 691 | 692 | // ZG_OFFS_TC register 693 | int8_t getZGyroOffsetTC(); 694 | void setZGyroOffsetTC(int8_t offset); 695 | 696 | // X_FINE_GAIN register 697 | int8_t getXFineGain(); 698 | void setXFineGain(int8_t gain); 699 | 700 | // Y_FINE_GAIN register 701 | int8_t getYFineGain(); 702 | void setYFineGain(int8_t gain); 703 | 704 | // Z_FINE_GAIN register 705 | int8_t getZFineGain(); 706 | void setZFineGain(int8_t gain); 707 | 708 | // XA_OFFS_* registers 709 | int16_t getXAccelOffset(); 710 | void setXAccelOffset(int16_t offset); 711 | 712 | // YA_OFFS_* register 713 | int16_t getYAccelOffset(); 714 | void setYAccelOffset(int16_t offset); 715 | 716 | // ZA_OFFS_* register 717 | int16_t getZAccelOffset(); 718 | void setZAccelOffset(int16_t offset); 719 | 720 | // XG_OFFS_USR* registers 721 | int16_t getXGyroOffset(); 722 | void setXGyroOffset(int16_t offset); 723 | 724 | // YG_OFFS_USR* register 725 | int16_t getYGyroOffset(); 726 | void setYGyroOffset(int16_t offset); 727 | 728 | // ZG_OFFS_USR* register 729 | int16_t getZGyroOffset(); 730 | void setZGyroOffset(int16_t offset); 731 | 732 | // INT_ENABLE register (DMP functions) 733 | bool getIntPLLReadyEnabled(); 734 | void setIntPLLReadyEnabled(bool enabled); 735 | bool getIntDMPEnabled(); 736 | void setIntDMPEnabled(bool enabled); 737 | 738 | // DMP_INT_STATUS 739 | bool getDMPInt5Status(); 740 | bool getDMPInt4Status(); 741 | bool getDMPInt3Status(); 742 | bool getDMPInt2Status(); 743 | bool getDMPInt1Status(); 744 | bool getDMPInt0Status(); 745 | 746 | // INT_STATUS register (DMP functions) 747 | bool getIntPLLReadyStatus(); 748 | bool getIntDMPStatus(); 749 | 750 | // USER_CTRL register (DMP functions) 751 | bool getDMPEnabled(); 752 | void setDMPEnabled(bool enabled); 753 | void resetDMP(); 754 | 755 | // BANK_SEL register 756 | void setMemoryBank(uint8_t bank, bool prefetchEnabled=false, bool userBank=false); 757 | 758 | // MEM_START_ADDR register 759 | void setMemoryStartAddress(uint8_t address); 760 | 761 | // MEM_R_W register 762 | uint8_t readMemoryByte(); 763 | void writeMemoryByte(uint8_t data); 764 | void readMemoryBlock(uint8_t *data, uint16_t dataSize, uint8_t bank=0, uint8_t address=0); 765 | bool writeMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank=0, uint8_t address=0, bool verify=true, bool useProgMem=false); 766 | bool writeProgMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank=0, uint8_t address=0, bool verify=true); 767 | 768 | bool writeDMPConfigurationSet(const uint8_t *data, uint16_t dataSize, bool useProgMem=false); 769 | bool writeProgDMPConfigurationSet(const uint8_t *data, uint16_t dataSize); 770 | 771 | // DMP_CFG_1 register 772 | uint8_t getDMPConfig1(); 773 | void setDMPConfig1(uint8_t config); 774 | 775 | // DMP_CFG_2 register 776 | uint8_t getDMPConfig2(); 777 | void setDMPConfig2(uint8_t config); 778 | 779 | // special methods for MotionApps 2.0 implementation 780 | #ifdef MPU6050_INCLUDE_DMP_MOTIONAPPS20 781 | uint8_t *dmpPacketBuffer; 782 | uint16_t dmpPacketSize; 783 | 784 | uint8_t dmpInitialize(); 785 | bool dmpPacketAvailable(); 786 | 787 | uint8_t dmpSetFIFORate(uint8_t fifoRate); 788 | uint8_t dmpGetFIFORate(); 789 | uint8_t dmpGetSampleStepSizeMS(); 790 | uint8_t dmpGetSampleFrequency(); 791 | int32_t dmpDecodeTemperature(int8_t tempReg); 792 | 793 | // Register callbacks after a packet of FIFO data is processed 794 | //uint8_t dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority); 795 | //uint8_t dmpUnregisterFIFORateProcess(inv_obj_func func); 796 | uint8_t dmpRunFIFORateProcesses(); 797 | 798 | // Setup FIFO for various output 799 | uint8_t dmpSendQuaternion(uint_fast16_t accuracy); 800 | uint8_t dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy); 801 | uint8_t dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy); 802 | uint8_t dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy); 803 | uint8_t dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy); 804 | uint8_t dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy); 805 | uint8_t dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy); 806 | uint8_t dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy); 807 | uint8_t dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy); 808 | uint8_t dmpSendPacketNumber(uint_fast16_t accuracy); 809 | uint8_t dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy); 810 | uint8_t dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy); 811 | 812 | // Get Fixed Point data from FIFO 813 | uint8_t dmpGetAccel(int32_t *data, const uint8_t* packet=0); 814 | uint8_t dmpGetAccel(int16_t *data, const uint8_t* packet=0); 815 | uint8_t dmpGetAccel(VectorInt16 *v, const uint8_t* packet=0); 816 | uint8_t dmpGetQuaternion(int32_t *data, const uint8_t* packet=0); 817 | uint8_t dmpGetQuaternion(int16_t *data, const uint8_t* packet=0); 818 | uint8_t dmpGetQuaternion(Quaternion *q, const uint8_t* packet=0); 819 | uint8_t dmpGet6AxisQuaternion(int32_t *data, const uint8_t* packet=0); 820 | uint8_t dmpGet6AxisQuaternion(int16_t *data, const uint8_t* packet=0); 821 | uint8_t dmpGet6AxisQuaternion(Quaternion *q, const uint8_t* packet=0); 822 | uint8_t dmpGetRelativeQuaternion(int32_t *data, const uint8_t* packet=0); 823 | uint8_t dmpGetRelativeQuaternion(int16_t *data, const uint8_t* packet=0); 824 | uint8_t dmpGetRelativeQuaternion(Quaternion *data, const uint8_t* packet=0); 825 | uint8_t dmpGetGyro(int32_t *data, const uint8_t* packet=0); 826 | uint8_t dmpGetGyro(int16_t *data, const uint8_t* packet=0); 827 | uint8_t dmpGetGyro(VectorInt16 *v, const uint8_t* packet=0); 828 | uint8_t dmpSetLinearAccelFilterCoefficient(float coef); 829 | uint8_t dmpGetLinearAccel(int32_t *data, const uint8_t* packet=0); 830 | uint8_t dmpGetLinearAccel(int16_t *data, const uint8_t* packet=0); 831 | uint8_t dmpGetLinearAccel(VectorInt16 *v, const uint8_t* packet=0); 832 | uint8_t dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity); 833 | uint8_t dmpGetLinearAccelInWorld(int32_t *data, const uint8_t* packet=0); 834 | uint8_t dmpGetLinearAccelInWorld(int16_t *data, const uint8_t* packet=0); 835 | uint8_t dmpGetLinearAccelInWorld(VectorInt16 *v, const uint8_t* packet=0); 836 | uint8_t dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q); 837 | uint8_t dmpGetGyroAndAccelSensor(int32_t *data, const uint8_t* packet=0); 838 | uint8_t dmpGetGyroAndAccelSensor(int16_t *data, const uint8_t* packet=0); 839 | uint8_t dmpGetGyroAndAccelSensor(VectorInt16 *g, VectorInt16 *a, const uint8_t* packet=0); 840 | uint8_t dmpGetGyroSensor(int32_t *data, const uint8_t* packet=0); 841 | uint8_t dmpGetGyroSensor(int16_t *data, const uint8_t* packet=0); 842 | uint8_t dmpGetGyroSensor(VectorInt16 *v, const uint8_t* packet=0); 843 | uint8_t dmpGetControlData(int32_t *data, const uint8_t* packet=0); 844 | uint8_t dmpGetTemperature(int32_t *data, const uint8_t* packet=0); 845 | uint8_t dmpGetGravity(int32_t *data, const uint8_t* packet=0); 846 | uint8_t dmpGetGravity(int16_t *data, const uint8_t* packet=0); 847 | uint8_t dmpGetGravity(VectorInt16 *v, const uint8_t* packet=0); 848 | uint8_t dmpGetGravity(VectorFloat *v, Quaternion *q); 849 | uint8_t dmpGetUnquantizedAccel(int32_t *data, const uint8_t* packet=0); 850 | uint8_t dmpGetUnquantizedAccel(int16_t *data, const uint8_t* packet=0); 851 | uint8_t dmpGetUnquantizedAccel(VectorInt16 *v, const uint8_t* packet=0); 852 | uint8_t dmpGetQuantizedAccel(int32_t *data, const uint8_t* packet=0); 853 | uint8_t dmpGetQuantizedAccel(int16_t *data, const uint8_t* packet=0); 854 | uint8_t dmpGetQuantizedAccel(VectorInt16 *v, const uint8_t* packet=0); 855 | uint8_t dmpGetExternalSensorData(int32_t *data, uint16_t size, const uint8_t* packet=0); 856 | uint8_t dmpGetEIS(int32_t *data, const uint8_t* packet=0); 857 | 858 | uint8_t dmpGetEuler(float *data, Quaternion *q); 859 | uint8_t dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity); 860 | 861 | // Get Floating Point data from FIFO 862 | uint8_t dmpGetAccelFloat(float *data, const uint8_t* packet=0); 863 | uint8_t dmpGetQuaternionFloat(float *data, const uint8_t* packet=0); 864 | 865 | uint8_t dmpProcessFIFOPacket(const unsigned char *dmpData); 866 | uint8_t dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed=NULL); 867 | 868 | uint8_t dmpSetFIFOProcessedCallback(void (*func) (void)); 869 | 870 | uint8_t dmpInitFIFOParam(); 871 | uint8_t dmpCloseFIFO(); 872 | uint8_t dmpSetGyroDataSource(uint8_t source); 873 | uint8_t dmpDecodeQuantizedAccel(); 874 | uint32_t dmpGetGyroSumOfSquare(); 875 | uint32_t dmpGetAccelSumOfSquare(); 876 | void dmpOverrideQuaternion(long *q); 877 | uint16_t dmpGetFIFOPacketSize(); 878 | #endif 879 | 880 | // special methods for MotionApps 4.1 implementation 881 | #ifdef MPU6050_INCLUDE_DMP_MOTIONAPPS41 882 | uint8_t *dmpPacketBuffer; 883 | uint16_t dmpPacketSize; 884 | 885 | uint8_t dmpInitialize(); 886 | bool dmpPacketAvailable(); 887 | 888 | uint8_t dmpSetFIFORate(uint8_t fifoRate); 889 | uint8_t dmpGetFIFORate(); 890 | uint8_t dmpGetSampleStepSizeMS(); 891 | uint8_t dmpGetSampleFrequency(); 892 | int32_t dmpDecodeTemperature(int8_t tempReg); 893 | 894 | // Register callbacks after a packet of FIFO data is processed 895 | //uint8_t dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority); 896 | //uint8_t dmpUnregisterFIFORateProcess(inv_obj_func func); 897 | uint8_t dmpRunFIFORateProcesses(); 898 | 899 | // Setup FIFO for various output 900 | uint8_t dmpSendQuaternion(uint_fast16_t accuracy); 901 | uint8_t dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy); 902 | uint8_t dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy); 903 | uint8_t dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy); 904 | uint8_t dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy); 905 | uint8_t dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy); 906 | uint8_t dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy); 907 | uint8_t dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy); 908 | uint8_t dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy); 909 | uint8_t dmpSendPacketNumber(uint_fast16_t accuracy); 910 | uint8_t dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy); 911 | uint8_t dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy); 912 | 913 | // Get Fixed Point data from FIFO 914 | uint8_t dmpGetAccel(int32_t *data, const uint8_t* packet=0); 915 | uint8_t dmpGetAccel(int16_t *data, const uint8_t* packet=0); 916 | uint8_t dmpGetAccel(VectorInt16 *v, const uint8_t* packet=0); 917 | uint8_t dmpGetQuaternion(int32_t *data, const uint8_t* packet=0); 918 | uint8_t dmpGetQuaternion(int16_t *data, const uint8_t* packet=0); 919 | uint8_t dmpGetQuaternion(Quaternion *q, const uint8_t* packet=0); 920 | uint8_t dmpGet6AxisQuaternion(int32_t *data, const uint8_t* packet=0); 921 | uint8_t dmpGet6AxisQuaternion(int16_t *data, const uint8_t* packet=0); 922 | uint8_t dmpGet6AxisQuaternion(Quaternion *q, const uint8_t* packet=0); 923 | uint8_t dmpGetRelativeQuaternion(int32_t *data, const uint8_t* packet=0); 924 | uint8_t dmpGetRelativeQuaternion(int16_t *data, const uint8_t* packet=0); 925 | uint8_t dmpGetRelativeQuaternion(Quaternion *data, const uint8_t* packet=0); 926 | uint8_t dmpGetGyro(int32_t *data, const uint8_t* packet=0); 927 | uint8_t dmpGetGyro(int16_t *data, const uint8_t* packet=0); 928 | uint8_t dmpGetGyro(VectorInt16 *v, const uint8_t* packet=0); 929 | uint8_t dmpGetMag(int16_t *data, const uint8_t* packet=0); 930 | uint8_t dmpSetLinearAccelFilterCoefficient(float coef); 931 | uint8_t dmpGetLinearAccel(int32_t *data, const uint8_t* packet=0); 932 | uint8_t dmpGetLinearAccel(int16_t *data, const uint8_t* packet=0); 933 | uint8_t dmpGetLinearAccel(VectorInt16 *v, const uint8_t* packet=0); 934 | uint8_t dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity); 935 | uint8_t dmpGetLinearAccelInWorld(int32_t *data, const uint8_t* packet=0); 936 | uint8_t dmpGetLinearAccelInWorld(int16_t *data, const uint8_t* packet=0); 937 | uint8_t dmpGetLinearAccelInWorld(VectorInt16 *v, const uint8_t* packet=0); 938 | uint8_t dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q); 939 | uint8_t dmpGetGyroAndAccelSensor(int32_t *data, const uint8_t* packet=0); 940 | uint8_t dmpGetGyroAndAccelSensor(int16_t *data, const uint8_t* packet=0); 941 | uint8_t dmpGetGyroAndAccelSensor(VectorInt16 *g, VectorInt16 *a, const uint8_t* packet=0); 942 | uint8_t dmpGetGyroSensor(int32_t *data, const uint8_t* packet=0); 943 | uint8_t dmpGetGyroSensor(int16_t *data, const uint8_t* packet=0); 944 | uint8_t dmpGetGyroSensor(VectorInt16 *v, const uint8_t* packet=0); 945 | uint8_t dmpGetControlData(int32_t *data, const uint8_t* packet=0); 946 | uint8_t dmpGetTemperature(int32_t *data, const uint8_t* packet=0); 947 | uint8_t dmpGetGravity(int32_t *data, const uint8_t* packet=0); 948 | uint8_t dmpGetGravity(int16_t *data, const uint8_t* packet=0); 949 | uint8_t dmpGetGravity(VectorInt16 *v, const uint8_t* packet=0); 950 | uint8_t dmpGetGravity(VectorFloat *v, Quaternion *q); 951 | uint8_t dmpGetUnquantizedAccel(int32_t *data, const uint8_t* packet=0); 952 | uint8_t dmpGetUnquantizedAccel(int16_t *data, const uint8_t* packet=0); 953 | uint8_t dmpGetUnquantizedAccel(VectorInt16 *v, const uint8_t* packet=0); 954 | uint8_t dmpGetQuantizedAccel(int32_t *data, const uint8_t* packet=0); 955 | uint8_t dmpGetQuantizedAccel(int16_t *data, const uint8_t* packet=0); 956 | uint8_t dmpGetQuantizedAccel(VectorInt16 *v, const uint8_t* packet=0); 957 | uint8_t dmpGetExternalSensorData(int32_t *data, uint16_t size, const uint8_t* packet=0); 958 | uint8_t dmpGetEIS(int32_t *data, const uint8_t* packet=0); 959 | 960 | uint8_t dmpGetEuler(float *data, Quaternion *q); 961 | uint8_t dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity); 962 | 963 | // Get Floating Point data from FIFO 964 | uint8_t dmpGetAccelFloat(float *data, const uint8_t* packet=0); 965 | uint8_t dmpGetQuaternionFloat(float *data, const uint8_t* packet=0); 966 | 967 | uint8_t dmpProcessFIFOPacket(const unsigned char *dmpData); 968 | uint8_t dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed=NULL); 969 | 970 | uint8_t dmpSetFIFOProcessedCallback(void (*func) (void)); 971 | 972 | uint8_t dmpInitFIFOParam(); 973 | uint8_t dmpCloseFIFO(); 974 | uint8_t dmpSetGyroDataSource(uint8_t source); 975 | uint8_t dmpDecodeQuantizedAccel(); 976 | uint32_t dmpGetGyroSumOfSquare(); 977 | uint32_t dmpGetAccelSumOfSquare(); 978 | void dmpOverrideQuaternion(long *q); 979 | uint16_t dmpGetFIFOPacketSize(); 980 | #endif 981 | 982 | private: 983 | uint8_t devAddr; 984 | uint8_t buffer[14]; 985 | }; 986 | 987 | #endif /* _MPU6050_H_ */ 988 | -------------------------------------------------------------------------------- /dev/MPU6050/MPU6050_9Axis_MotionApps41.h: -------------------------------------------------------------------------------- 1 | // I2Cdev library collection - MPU6050 I2C device class, 9-axis MotionApps 4.1 implementation 2 | // Based on InvenSense MPU-6050 register map document rev. 2.0, 5/19/2011 (RM-MPU-6000A-00) 3 | // 6/18/2012 by Jeff Rowberg 4 | // Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib 5 | // 6 | // Changelog: 7 | // ... - ongoing debug release 8 | 9 | /* ============================================ 10 | I2Cdev device library code is placed under the MIT license 11 | Copyright (c) 2012 Jeff Rowberg 12 | 13 | Permission is hereby granted, free of charge, to any person obtaining a copy 14 | of this software and associated documentation files (the "Software"), to deal 15 | in the Software without restriction, including without limitation the rights 16 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 17 | copies of the Software, and to permit persons to whom the Software is 18 | furnished to do so, subject to the following conditions: 19 | 20 | The above copyright notice and this permission notice shall be included in 21 | all copies or substantial portions of the Software. 22 | 23 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 24 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 25 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 26 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 27 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 28 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 29 | THE SOFTWARE. 30 | =============================================== 31 | */ 32 | 33 | #ifndef _MPU6050_9AXIS_MOTIONAPPS41_H_ 34 | #define _MPU6050_9AXIS_MOTIONAPPS41_H_ 35 | 36 | #include "I2Cdev.h" 37 | #include "helper_3dmath.h" 38 | 39 | // MotionApps 4.1 DMP implementation, built using the MPU-9150 "MotionFit" board 40 | #define MPU6050_INCLUDE_DMP_MOTIONAPPS41 41 | 42 | #include "MPU6050.h" 43 | 44 | // Tom Carpenter's conditional PROGMEM code 45 | // http://forum.arduino.cc/index.php?topic=129407.0 46 | #ifndef __arm__ 47 | #include 48 | #else 49 | // Teensy 3.0 library conditional PROGMEM code from Paul Stoffregen 50 | #ifndef __PGMSPACE_H_ 51 | #define __PGMSPACE_H_ 1 52 | #include 53 | 54 | #define PROGMEM 55 | #define PGM_P const char * 56 | #define PSTR(str) (str) 57 | #define F(x) x 58 | 59 | typedef void prog_void; 60 | typedef char prog_char; 61 | typedef unsigned char prog_uchar; 62 | typedef int8_t prog_int8_t; 63 | typedef uint8_t prog_uint8_t; 64 | typedef int16_t prog_int16_t; 65 | typedef uint16_t prog_uint16_t; 66 | typedef int32_t prog_int32_t; 67 | typedef uint32_t prog_uint32_t; 68 | 69 | #define strcpy_P(dest, src) strcpy((dest), (src)) 70 | #define strcat_P(dest, src) strcat((dest), (src)) 71 | #define strcmp_P(a, b) strcmp((a), (b)) 72 | 73 | #define pgm_read_byte(addr) (*(const unsigned char *)(addr)) 74 | #define pgm_read_word(addr) (*(const unsigned short *)(addr)) 75 | #define pgm_read_dword(addr) (*(const unsigned long *)(addr)) 76 | #define pgm_read_float(addr) (*(const float *)(addr)) 77 | 78 | #define pgm_read_byte_near(addr) pgm_read_byte(addr) 79 | #define pgm_read_word_near(addr) pgm_read_word(addr) 80 | #define pgm_read_dword_near(addr) pgm_read_dword(addr) 81 | #define pgm_read_float_near(addr) pgm_read_float(addr) 82 | #define pgm_read_byte_far(addr) pgm_read_byte(addr) 83 | #define pgm_read_word_far(addr) pgm_read_word(addr) 84 | #define pgm_read_dword_far(addr) pgm_read_dword(addr) 85 | #define pgm_read_float_far(addr) pgm_read_float(addr) 86 | #endif 87 | #endif 88 | 89 | // NOTE! Enabling DEBUG adds about 3.3kB to the flash program size. 90 | // Debug output is now working even on ATMega328P MCUs (e.g. Arduino Uno) 91 | // after moving string constants to flash memory storage using the F() 92 | // compiler macro (Arduino IDE 1.0+ required). 93 | 94 | //#define DEBUG 95 | #ifdef DEBUG 96 | #define DEBUG_PRINT(x) Serial.print(x) 97 | #define DEBUG_PRINTF(x, y) Serial.print(x, y) 98 | #define DEBUG_PRINTLN(x) Serial.println(x) 99 | #define DEBUG_PRINTLNF(x, y) Serial.println(x, y) 100 | #else 101 | #define DEBUG_PRINT(x) 102 | #define DEBUG_PRINTF(x, y) 103 | #define DEBUG_PRINTLN(x) 104 | #define DEBUG_PRINTLNF(x, y) 105 | #endif 106 | 107 | #define MPU6050_DMP_CODE_SIZE 1962 // dmpMemory[] 108 | #define MPU6050_DMP_CONFIG_SIZE 232 // dmpConfig[] 109 | #define MPU6050_DMP_UPDATES_SIZE 140 // dmpUpdates[] 110 | 111 | /* ================================================================================================ * 112 | | Default MotionApps v4.1 48-byte FIFO packet structure: | 113 | | | 114 | | [QUAT W][ ][QUAT X][ ][QUAT Y][ ][QUAT Z][ ][GYRO X][ ][GYRO Y][ ] | 115 | | 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 | 116 | | | 117 | | [GYRO Z][ ][MAG X ][MAG Y ][MAG Z ][ACC X ][ ][ACC Y ][ ][ACC Z ][ ][ ] | 118 | | 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 | 119 | * ================================================================================================ */ 120 | 121 | // this block of memory gets written to the MPU on start-up, and it seems 122 | // to be volatile memory, so it has to be done each time (it only takes ~1 123 | // second though) 124 | const prog_uchar dmpMemory[MPU6050_DMP_CODE_SIZE] PROGMEM = { 125 | // bank 0, 256 bytes 126 | 0xFB, 0x00, 0x00, 0x3E, 0x00, 0x0B, 0x00, 0x36, 0x00, 0x01, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00, 127 | 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0xFA, 0x80, 0x00, 0x0B, 0x12, 0x82, 0x00, 0x01, 128 | 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 129 | 0x00, 0x28, 0x00, 0x00, 0xFF, 0xFF, 0x45, 0x81, 0xFF, 0xFF, 0xFA, 0x72, 0x00, 0x00, 0x00, 0x00, 130 | 0x00, 0x00, 0x03, 0xE8, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x7F, 0xFF, 0xFF, 0xFE, 0x80, 0x01, 131 | 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 132 | 0x00, 0x3E, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xCA, 0xE3, 0x09, 0x3E, 0x80, 0x00, 0x00, 133 | 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00, 134 | 0x41, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x0B, 0x2A, 0x00, 0x00, 0x16, 0x55, 0x00, 0x00, 0x21, 0x82, 135 | 0xFD, 0x87, 0x26, 0x50, 0xFD, 0x80, 0x00, 0x00, 0x00, 0x1F, 0x00, 0x00, 0x00, 0x05, 0x80, 0x00, 136 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 137 | 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x6F, 0x00, 0x02, 0x65, 0x32, 0x00, 0x00, 0x5E, 0xC0, 138 | 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 139 | 0xFB, 0x8C, 0x6F, 0x5D, 0xFD, 0x5D, 0x08, 0xD9, 0x00, 0x7C, 0x73, 0x3B, 0x00, 0x6C, 0x12, 0xCC, 140 | 0x32, 0x00, 0x13, 0x9D, 0x32, 0x00, 0xD0, 0xD6, 0x32, 0x00, 0x08, 0x00, 0x40, 0x00, 0x01, 0xF4, 141 | 0xFF, 0xE6, 0x80, 0x79, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0xD0, 0xD6, 0x00, 0x00, 0x27, 0x10, 142 | 143 | // bank 1, 256 bytes 144 | 0xFB, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 145 | 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x00, 146 | 0x00, 0x00, 0xFA, 0x36, 0xFF, 0xBC, 0x30, 0x8E, 0x00, 0x05, 0xFB, 0xF0, 0xFF, 0xD9, 0x5B, 0xC8, 147 | 0xFF, 0xD0, 0x9A, 0xBE, 0x00, 0x00, 0x10, 0xA9, 0xFF, 0xF4, 0x1E, 0xB2, 0x00, 0xCE, 0xBB, 0xF7, 148 | 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x04, 0x00, 0x02, 0x00, 0x02, 0x02, 0x00, 0x00, 0x0C, 149 | 0xFF, 0xC2, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0xCF, 0x80, 0x00, 0x40, 0x00, 0x00, 0x00, 150 | 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14, 151 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 152 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 153 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 154 | 0x00, 0x00, 0x00, 0x00, 0x03, 0x3F, 0x68, 0xB6, 0x79, 0x35, 0x28, 0xBC, 0xC6, 0x7E, 0xD1, 0x6C, 155 | 0x80, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0xB2, 0x6A, 0x00, 0x00, 0x00, 0x00, 156 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3F, 0xF0, 0x00, 0x00, 0x00, 0x30, 157 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 158 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 159 | 0x00, 0x00, 0x25, 0x4D, 0x00, 0x2F, 0x70, 0x6D, 0x00, 0x00, 0x05, 0xAE, 0x00, 0x0C, 0x02, 0xD0, 160 | 161 | // bank 2, 256 bytes 162 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 163 | 0x00, 0x00, 0x01, 0x00, 0x00, 0x44, 0x00, 0x00, 0x00, 0x00, 0x0C, 0x00, 0x00, 0x00, 0x01, 0x00, 164 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x54, 0x00, 0x00, 0xFF, 0xEF, 0x00, 0x00, 165 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 166 | 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 167 | 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 168 | 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 169 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 170 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 171 | 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 172 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 173 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 174 | 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 175 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 176 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x47, 0x78, 0xA2, 177 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 178 | 179 | // bank 3, 256 bytes 180 | 0xD8, 0xDC, 0xF4, 0xD8, 0xB9, 0xAB, 0xF3, 0xF8, 0xFA, 0xF1, 0xBA, 0xA2, 0xDE, 0xB2, 0xB8, 0xB4, 181 | 0xA8, 0x81, 0x98, 0xF7, 0x4A, 0x90, 0x7F, 0x91, 0x6A, 0xF3, 0xF9, 0xDB, 0xA8, 0xF9, 0xB0, 0xBA, 182 | 0xA0, 0x80, 0xF2, 0xCE, 0x81, 0xF3, 0xC2, 0xF1, 0xC1, 0xF2, 0xC3, 0xF3, 0xCC, 0xA2, 0xB2, 0x80, 183 | 0xF1, 0xC6, 0xD8, 0x80, 0xBA, 0xA7, 0xDF, 0xDF, 0xDF, 0xF2, 0xA7, 0xC3, 0xCB, 0xC5, 0xB6, 0xF0, 184 | 0x87, 0xA2, 0x94, 0x24, 0x48, 0x70, 0x3C, 0x95, 0x40, 0x68, 0x34, 0x58, 0x9B, 0x78, 0xA2, 0xF1, 185 | 0x83, 0x92, 0x2D, 0x55, 0x7D, 0xD8, 0xB1, 0xB4, 0xB8, 0xA1, 0xD0, 0x91, 0x80, 0xF2, 0x70, 0xF3, 186 | 0x70, 0xF2, 0x7C, 0x80, 0xA8, 0xF1, 0x01, 0xB0, 0x98, 0x87, 0xD9, 0x43, 0xD8, 0x86, 0xC9, 0x88, 187 | 0xBA, 0xA1, 0xF2, 0x0E, 0xB8, 0x97, 0x80, 0xF1, 0xA9, 0xDF, 0xDF, 0xDF, 0xAA, 0xDF, 0xDF, 0xDF, 188 | 0xF2, 0xAA, 0xC5, 0xCD, 0xC7, 0xA9, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97, 0x97, 0xF1, 0xA9, 0x89, 189 | 0x26, 0x46, 0x66, 0xB0, 0xB4, 0xBA, 0x80, 0xAC, 0xDE, 0xF2, 0xCA, 0xF1, 0xB2, 0x8C, 0x02, 0xA9, 190 | 0xB6, 0x98, 0x00, 0x89, 0x0E, 0x16, 0x1E, 0xB8, 0xA9, 0xB4, 0x99, 0x2C, 0x54, 0x7C, 0xB0, 0x8A, 191 | 0xA8, 0x96, 0x36, 0x56, 0x76, 0xF1, 0xB9, 0xAF, 0xB4, 0xB0, 0x83, 0xC0, 0xB8, 0xA8, 0x97, 0x11, 192 | 0xB1, 0x8F, 0x98, 0xB9, 0xAF, 0xF0, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xF1, 0xA3, 0x29, 0x55, 193 | 0x7D, 0xAF, 0x83, 0xB5, 0x93, 0xF0, 0x00, 0x28, 0x50, 0xF5, 0xBA, 0xAD, 0x8F, 0x9F, 0x28, 0x54, 194 | 0x7C, 0xB9, 0xF1, 0xA3, 0x86, 0x9F, 0x61, 0xA6, 0xDA, 0xDE, 0xDF, 0xDB, 0xB2, 0xB6, 0x8E, 0x9D, 195 | 0xAE, 0xF5, 0x60, 0x68, 0x70, 0xB1, 0xB5, 0xF1, 0xDA, 0xA6, 0xDF, 0xD9, 0xA6, 0xFA, 0xA3, 0x86, 196 | 197 | // bank 4, 256 bytes 198 | 0x96, 0xDB, 0x31, 0xA6, 0xD9, 0xF8, 0xDF, 0xBA, 0xA6, 0x8F, 0xC2, 0xC5, 0xC7, 0xB2, 0x8C, 0xC1, 199 | 0xB8, 0xA2, 0xDF, 0xDF, 0xDF, 0xA3, 0xDF, 0xDF, 0xDF, 0xD8, 0xD8, 0xF1, 0xB8, 0xA8, 0xB2, 0x86, 200 | 0xB4, 0x98, 0x0D, 0x35, 0x5D, 0xB8, 0xAA, 0x98, 0xB0, 0x87, 0x2D, 0x35, 0x3D, 0xB2, 0xB6, 0xBA, 201 | 0xAF, 0x8C, 0x96, 0x19, 0x8F, 0x9F, 0xA7, 0x0E, 0x16, 0x1E, 0xB4, 0x9A, 0xB8, 0xAA, 0x87, 0x2C, 202 | 0x54, 0x7C, 0xB9, 0xA3, 0xDE, 0xDF, 0xDF, 0xA3, 0xB1, 0x80, 0xF2, 0xC4, 0xCD, 0xC9, 0xF1, 0xB8, 203 | 0xA9, 0xB4, 0x99, 0x83, 0x0D, 0x35, 0x5D, 0x89, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0xB5, 0x93, 0xA3, 204 | 0x0E, 0x16, 0x1E, 0xA9, 0x2C, 0x54, 0x7C, 0xB8, 0xB4, 0xB0, 0xF1, 0x97, 0x83, 0xA8, 0x11, 0x84, 205 | 0xA5, 0x09, 0x98, 0xA3, 0x83, 0xF0, 0xDA, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xD8, 0xF1, 0xA5, 206 | 0x29, 0x55, 0x7D, 0xA5, 0x85, 0x95, 0x02, 0x1A, 0x2E, 0x3A, 0x56, 0x5A, 0x40, 0x48, 0xF9, 0xF3, 207 | 0xA3, 0xD9, 0xF8, 0xF0, 0x98, 0x83, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0x97, 0x82, 0xA8, 0xF1, 208 | 0x11, 0xF0, 0x98, 0xA2, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xDA, 0xF3, 0xDE, 0xD8, 0x83, 0xA5, 209 | 0x94, 0x01, 0xD9, 0xA3, 0x02, 0xF1, 0xA2, 0xC3, 0xC5, 0xC7, 0xD8, 0xF1, 0x84, 0x92, 0xA2, 0x4D, 210 | 0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9, 211 | 0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0x93, 0xA3, 0x4D, 212 | 0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9, 213 | 0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0xA8, 0x8A, 0x9A, 214 | 215 | // bank 5, 256 bytes 216 | 0xF0, 0x28, 0x50, 0x78, 0x9E, 0xF3, 0x88, 0x18, 0xF1, 0x9F, 0x1D, 0x98, 0xA8, 0xD9, 0x08, 0xD8, 217 | 0xC8, 0x9F, 0x12, 0x9E, 0xF3, 0x15, 0xA8, 0xDA, 0x12, 0x10, 0xD8, 0xF1, 0xAF, 0xC8, 0x97, 0x87, 218 | 0x34, 0xB5, 0xB9, 0x94, 0xA4, 0x21, 0xF3, 0xD9, 0x22, 0xD8, 0xF2, 0x2D, 0xF3, 0xD9, 0x2A, 0xD8, 219 | 0xF2, 0x35, 0xF3, 0xD9, 0x32, 0xD8, 0x81, 0xA4, 0x60, 0x60, 0x61, 0xD9, 0x61, 0xD8, 0x6C, 0x68, 220 | 0x69, 0xD9, 0x69, 0xD8, 0x74, 0x70, 0x71, 0xD9, 0x71, 0xD8, 0xB1, 0xA3, 0x84, 0x19, 0x3D, 0x5D, 221 | 0xA3, 0x83, 0x1A, 0x3E, 0x5E, 0x93, 0x10, 0x30, 0x81, 0x10, 0x11, 0xB8, 0xB0, 0xAF, 0x8F, 0x94, 222 | 0xF2, 0xDA, 0x3E, 0xD8, 0xB4, 0x9A, 0xA8, 0x87, 0x29, 0xDA, 0xF8, 0xD8, 0x87, 0x9A, 0x35, 0xDA, 223 | 0xF8, 0xD8, 0x87, 0x9A, 0x3D, 0xDA, 0xF8, 0xD8, 0xB1, 0xB9, 0xA4, 0x98, 0x85, 0x02, 0x2E, 0x56, 224 | 0xA5, 0x81, 0x00, 0x0C, 0x14, 0xA3, 0x97, 0xB0, 0x8A, 0xF1, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9, 225 | 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x84, 0x0D, 0xDA, 0x0E, 0xD8, 0xA3, 0x29, 0x83, 0xDA, 226 | 0x2C, 0x0E, 0xD8, 0xA3, 0x84, 0x49, 0x83, 0xDA, 0x2C, 0x4C, 0x0E, 0xD8, 0xB8, 0xB0, 0x97, 0x86, 227 | 0xA8, 0x31, 0x9B, 0x06, 0x99, 0x07, 0xAB, 0x97, 0x28, 0x88, 0x9B, 0xF0, 0x0C, 0x20, 0x14, 0x40, 228 | 0xB9, 0xA3, 0x8A, 0xC3, 0xC5, 0xC7, 0x9A, 0xA3, 0x28, 0x50, 0x78, 0xF1, 0xB5, 0x93, 0x01, 0xD9, 229 | 0xDF, 0xDF, 0xDF, 0xD8, 0xB8, 0xB4, 0xA8, 0x8C, 0x9C, 0xF0, 0x04, 0x28, 0x51, 0x79, 0x1D, 0x30, 230 | 0x14, 0x38, 0xB2, 0x82, 0xAB, 0xD0, 0x98, 0x2C, 0x50, 0x50, 0x78, 0x78, 0x9B, 0xF1, 0x1A, 0xB0, 231 | 0xF0, 0xB1, 0x83, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0xB0, 0x8B, 0x29, 0x51, 0x79, 0xB1, 0x83, 0x24, 232 | 233 | // bank 6, 256 bytes 234 | 0x70, 0x59, 0xB0, 0x8B, 0x20, 0x58, 0x71, 0xB1, 0x83, 0x44, 0x69, 0x38, 0xB0, 0x8B, 0x39, 0x40, 235 | 0x68, 0xB1, 0x83, 0x64, 0x48, 0x31, 0xB0, 0x8B, 0x30, 0x49, 0x60, 0xA5, 0x88, 0x20, 0x09, 0x71, 236 | 0x58, 0x44, 0x68, 0x11, 0x39, 0x64, 0x49, 0x30, 0x19, 0xF1, 0xAC, 0x00, 0x2C, 0x54, 0x7C, 0xF0, 237 | 0x8C, 0xA8, 0x04, 0x28, 0x50, 0x78, 0xF1, 0x88, 0x97, 0x26, 0xA8, 0x59, 0x98, 0xAC, 0x8C, 0x02, 238 | 0x26, 0x46, 0x66, 0xF0, 0x89, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 239 | 0x64, 0x48, 0x31, 0xA9, 0x88, 0x09, 0x20, 0x59, 0x70, 0xAB, 0x11, 0x38, 0x40, 0x69, 0xA8, 0x19, 240 | 0x31, 0x48, 0x60, 0x8C, 0xA8, 0x3C, 0x41, 0x5C, 0x20, 0x7C, 0x00, 0xF1, 0x87, 0x98, 0x19, 0x86, 241 | 0xA8, 0x6E, 0x76, 0x7E, 0xA9, 0x99, 0x88, 0x2D, 0x55, 0x7D, 0x9E, 0xB9, 0xA3, 0x8A, 0x22, 0x8A, 242 | 0x6E, 0x8A, 0x56, 0x8A, 0x5E, 0x9F, 0xB1, 0x83, 0x06, 0x26, 0x46, 0x66, 0x0E, 0x2E, 0x4E, 0x6E, 243 | 0x9D, 0xB8, 0xAD, 0x00, 0x2C, 0x54, 0x7C, 0xF2, 0xB1, 0x8C, 0xB4, 0x99, 0xB9, 0xA3, 0x2D, 0x55, 244 | 0x7D, 0x81, 0x91, 0xAC, 0x38, 0xAD, 0x3A, 0xB5, 0x83, 0x91, 0xAC, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 245 | 0xD9, 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0x8C, 0x9D, 0xAE, 0x29, 0xD9, 0x04, 0xAE, 0xD8, 0x51, 246 | 0xD9, 0x04, 0xAE, 0xD8, 0x79, 0xD9, 0x04, 0xD8, 0x81, 0xF3, 0x9D, 0xAD, 0x00, 0x8D, 0xAE, 0x19, 247 | 0x81, 0xAD, 0xD9, 0x01, 0xD8, 0xF2, 0xAE, 0xDA, 0x26, 0xD8, 0x8E, 0x91, 0x29, 0x83, 0xA7, 0xD9, 248 | 0xAD, 0xAD, 0xAD, 0xAD, 0xF3, 0x2A, 0xD8, 0xD8, 0xF1, 0xB0, 0xAC, 0x89, 0x91, 0x3E, 0x5E, 0x76, 249 | 0xF3, 0xAC, 0x2E, 0x2E, 0xF1, 0xB1, 0x8C, 0x5A, 0x9C, 0xAC, 0x2C, 0x28, 0x28, 0x28, 0x9C, 0xAC, 250 | 251 | // bank 7, 170 bytes (remainder) 252 | 0x30, 0x18, 0xA8, 0x98, 0x81, 0x28, 0x34, 0x3C, 0x97, 0x24, 0xA7, 0x28, 0x34, 0x3C, 0x9C, 0x24, 253 | 0xF2, 0xB0, 0x89, 0xAC, 0x91, 0x2C, 0x4C, 0x6C, 0x8A, 0x9B, 0x2D, 0xD9, 0xD8, 0xD8, 0x51, 0xD9, 254 | 0xD8, 0xD8, 0x79, 0xD9, 0xD8, 0xD8, 0xF1, 0x9E, 0x88, 0xA3, 0x31, 0xDA, 0xD8, 0xD8, 0x91, 0x2D, 255 | 0xD9, 0x28, 0xD8, 0x4D, 0xD9, 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x83, 0x93, 0x35, 0x3D, 256 | 0x80, 0x25, 0xDA, 0xD8, 0xD8, 0x85, 0x69, 0xDA, 0xD8, 0xD8, 0xB4, 0x93, 0x81, 0xA3, 0x28, 0x34, 257 | 0x3C, 0xF3, 0xAB, 0x8B, 0xA3, 0x91, 0xB6, 0x09, 0xB4, 0xD9, 0xAB, 0xDE, 0xB0, 0x87, 0x9C, 0xB9, 258 | 0xA3, 0xDD, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x95, 0xF1, 0xA3, 0xA3, 0xA3, 0x9D, 0xF1, 0xA3, 0xA3, 259 | 0xA3, 0xA3, 0xF2, 0xA3, 0xB4, 0x90, 0x80, 0xF2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 260 | 0xA3, 0xA3, 0xB2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xB0, 0x87, 0xB5, 0x99, 0xF1, 0xA3, 0xA3, 261 | 0xA3, 0x98, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x97, 0xA3, 0xA3, 0xA3, 0xA3, 0xF3, 0x9B, 0xA3, 0xA3, 262 | 0xDC, 0xB9, 0xA7, 0xF1, 0x26, 0x26, 0x26, 0xD8, 0xD8, 0xFF 263 | }; 264 | 265 | const prog_uchar dmpConfig[MPU6050_DMP_CONFIG_SIZE] PROGMEM = { 266 | // BANK OFFSET LENGTH [DATA] 267 | 0x02, 0xEC, 0x04, 0x00, 0x47, 0x7D, 0x1A, // ? 268 | 0x03, 0x82, 0x03, 0x4C, 0xCD, 0x6C, // FCFG_1 inv_set_gyro_calibration 269 | 0x03, 0xB2, 0x03, 0x36, 0x56, 0x76, // FCFG_3 inv_set_gyro_calibration 270 | 0x00, 0x68, 0x04, 0x02, 0xCA, 0xE3, 0x09, // D_0_104 inv_set_gyro_calibration 271 | 0x01, 0x0C, 0x04, 0x00, 0x00, 0x00, 0x00, // D_1_152 inv_set_accel_calibration 272 | 0x03, 0x86, 0x03, 0x0C, 0xC9, 0x2C, // FCFG_2 inv_set_accel_calibration 273 | 0x03, 0x90, 0x03, 0x26, 0x46, 0x66, // (continued)...FCFG_2 inv_set_accel_calibration 274 | 0x00, 0x6C, 0x02, 0x40, 0x00, // D_0_108 inv_set_accel_calibration 275 | 276 | 0x02, 0x40, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_00 inv_set_compass_calibration 277 | 0x02, 0x44, 0x04, 0x40, 0x00, 0x00, 0x00, // CPASS_MTX_01 278 | 0x02, 0x48, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_02 279 | 0x02, 0x4C, 0x04, 0x40, 0x00, 0x00, 0x00, // CPASS_MTX_10 280 | 0x02, 0x50, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_11 281 | 0x02, 0x54, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_12 282 | 0x02, 0x58, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_20 283 | 0x02, 0x5C, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_21 284 | 0x02, 0xBC, 0x04, 0xC0, 0x00, 0x00, 0x00, // CPASS_MTX_22 285 | 286 | 0x01, 0xEC, 0x04, 0x00, 0x00, 0x40, 0x00, // D_1_236 inv_apply_endian_accel 287 | 0x03, 0x86, 0x06, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97, // FCFG_2 inv_set_mpu_sensors 288 | 0x04, 0x22, 0x03, 0x0D, 0x35, 0x5D, // CFG_MOTION_BIAS inv_turn_on_bias_from_no_motion 289 | 0x00, 0xA3, 0x01, 0x00, // ? 290 | 0x04, 0x29, 0x04, 0x87, 0x2D, 0x35, 0x3D, // FCFG_5 inv_set_bias_update 291 | 0x07, 0x62, 0x05, 0xF1, 0x20, 0x28, 0x30, 0x38, // CFG_8 inv_send_quaternion 292 | 0x07, 0x9F, 0x01, 0x30, // CFG_16 inv_set_footer 293 | 0x07, 0x67, 0x01, 0x9A, // CFG_GYRO_SOURCE inv_send_gyro 294 | 0x07, 0x68, 0x04, 0xF1, 0x28, 0x30, 0x38, // CFG_9 inv_send_gyro -> inv_construct3_fifo 295 | 0x07, 0x62, 0x05, 0xF1, 0x20, 0x28, 0x30, 0x38, // ? 296 | 0x02, 0x0C, 0x04, 0x00, 0x00, 0x00, 0x00, // ? 297 | 0x07, 0x83, 0x06, 0xC2, 0xCA, 0xC4, 0xA3, 0xA3, 0xA3, // ? 298 | // SPECIAL 0x01 = enable interrupts 299 | 0x00, 0x00, 0x00, 0x01, // SET INT_ENABLE, SPECIAL INSTRUCTION 300 | 0x07, 0xA7, 0x01, 0xFE, // ? 301 | 0x07, 0x62, 0x05, 0xF1, 0x20, 0x28, 0x30, 0x38, // ? 302 | 0x07, 0x67, 0x01, 0x9A, // ? 303 | 0x07, 0x68, 0x04, 0xF1, 0x28, 0x30, 0x38, // CFG_12 inv_send_accel -> inv_construct3_fifo 304 | 0x07, 0x8D, 0x04, 0xF1, 0x28, 0x30, 0x38, // ??? CFG_12 inv_send_mag -> inv_construct3_fifo 305 | 0x02, 0x16, 0x02, 0x00, 0x03 // D_0_22 inv_set_fifo_rate 306 | 307 | // This very last 0x01 WAS a 0x09, which drops the FIFO rate down to 20 Hz. 0x07 is 25 Hz, 308 | // 0x01 is 100Hz. Going faster than 100Hz (0x00=200Hz) tends to result in very noisy data. 309 | // DMP output frequency is calculated easily using this equation: (200Hz / (1 + value)) 310 | 311 | // It is important to make sure the host processor can keep up with reading and processing 312 | // the FIFO output at the desired rate. Handling FIFO overflow cleanly is also a good idea. 313 | }; 314 | 315 | const prog_uchar dmpUpdates[MPU6050_DMP_UPDATES_SIZE] PROGMEM = { 316 | 0x01, 0xB2, 0x02, 0xFF, 0xF5, 317 | 0x01, 0x90, 0x04, 0x0A, 0x0D, 0x97, 0xC0, 318 | 0x00, 0xA3, 0x01, 0x00, 319 | 0x04, 0x29, 0x04, 0x87, 0x2D, 0x35, 0x3D, 320 | 0x01, 0x6A, 0x02, 0x06, 0x00, 321 | 0x01, 0x60, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 322 | 0x00, 0x60, 0x04, 0x40, 0x00, 0x00, 0x00, 323 | 0x02, 0x60, 0x0C, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 324 | 0x01, 0x08, 0x02, 0x01, 0x20, 325 | 0x01, 0x0A, 0x02, 0x00, 0x4E, 326 | 0x01, 0x02, 0x02, 0xFE, 0xB3, 327 | 0x02, 0x6C, 0x04, 0x00, 0x00, 0x00, 0x00, // READ 328 | 0x02, 0x6C, 0x04, 0xFA, 0xFE, 0x00, 0x00, 329 | 0x02, 0x60, 0x0C, 0xFF, 0xFF, 0xCB, 0x4D, 0x00, 0x01, 0x08, 0xC1, 0xFF, 0xFF, 0xBC, 0x2C, 330 | 0x02, 0xF4, 0x04, 0x00, 0x00, 0x00, 0x00, 331 | 0x02, 0xF8, 0x04, 0x00, 0x00, 0x00, 0x00, 332 | 0x02, 0xFC, 0x04, 0x00, 0x00, 0x00, 0x00, 333 | 0x00, 0x60, 0x04, 0x40, 0x00, 0x00, 0x00, 334 | 0x00, 0x60, 0x04, 0x00, 0x40, 0x00, 0x00 335 | }; 336 | 337 | uint8_t MPU6050::dmpInitialize() { 338 | // reset device 339 | DEBUG_PRINTLN(F("\n\nResetting MPU6050...")); 340 | reset(); 341 | delay(30); // wait after reset 342 | 343 | // disable sleep mode 344 | DEBUG_PRINTLN(F("Disabling sleep mode...")); 345 | setSleepEnabled(false); 346 | 347 | // get MPU product ID 348 | DEBUG_PRINTLN(F("Getting product ID...")); 349 | //uint8_t productID = 0; //getProductID(); 350 | DEBUG_PRINT(F("Product ID = ")); 351 | DEBUG_PRINT(productID); 352 | 353 | // get MPU hardware revision 354 | DEBUG_PRINTLN(F("Selecting user bank 16...")); 355 | setMemoryBank(0x10, true, true); 356 | DEBUG_PRINTLN(F("Selecting memory byte 6...")); 357 | setMemoryStartAddress(0x06); 358 | DEBUG_PRINTLN(F("Checking hardware revision...")); 359 | uint8_t hwRevision = readMemoryByte(); 360 | DEBUG_PRINT(F("Revision @ user[16][6] = ")); 361 | DEBUG_PRINTLNF(hwRevision, HEX); 362 | DEBUG_PRINTLN(F("Resetting memory bank selection to 0...")); 363 | setMemoryBank(0, false, false); 364 | 365 | // check OTP bank valid 366 | DEBUG_PRINTLN(F("Reading OTP bank valid flag...")); 367 | uint8_t otpValid = getOTPBankValid(); 368 | DEBUG_PRINT(F("OTP bank is ")); 369 | DEBUG_PRINTLN(otpValid ? F("valid!") : F("invalid!")); 370 | 371 | // get X/Y/Z gyro offsets 372 | DEBUG_PRINTLN(F("Reading gyro offset values...")); 373 | int8_t xgOffset = getXGyroOffset(); 374 | int8_t ygOffset = getYGyroOffset(); 375 | int8_t zgOffset = getZGyroOffset(); 376 | DEBUG_PRINT(F("X gyro offset = ")); 377 | DEBUG_PRINTLN(xgOffset); 378 | DEBUG_PRINT(F("Y gyro offset = ")); 379 | DEBUG_PRINTLN(ygOffset); 380 | DEBUG_PRINT(F("Z gyro offset = ")); 381 | DEBUG_PRINTLN(zgOffset); 382 | 383 | I2Cdev::readByte(devAddr, MPU6050_RA_USER_CTRL, buffer); // ? 384 | 385 | DEBUG_PRINTLN(F("Enabling interrupt latch, clear on any read, AUX bypass enabled")); 386 | I2Cdev::writeByte(devAddr, MPU6050_RA_INT_PIN_CFG, 0x32); 387 | 388 | // enable MPU AUX I2C bypass mode 389 | //DEBUG_PRINTLN(F("Enabling AUX I2C bypass mode...")); 390 | //setI2CBypassEnabled(true); 391 | 392 | DEBUG_PRINTLN(F("Setting magnetometer mode to power-down...")); 393 | //mag -> setMode(0); 394 | I2Cdev::writeByte(0x0E, 0x0A, 0x00); 395 | 396 | DEBUG_PRINTLN(F("Setting magnetometer mode to fuse access...")); 397 | //mag -> setMode(0x0F); 398 | I2Cdev::writeByte(0x0E, 0x0A, 0x0F); 399 | 400 | DEBUG_PRINTLN(F("Reading mag magnetometer factory calibration...")); 401 | int8_t asax, asay, asaz; 402 | //mag -> getAdjustment(&asax, &asay, &asaz); 403 | I2Cdev::readBytes(0x0E, 0x10, 3, buffer); 404 | asax = (int8_t)buffer[0]; 405 | asay = (int8_t)buffer[1]; 406 | asaz = (int8_t)buffer[2]; 407 | DEBUG_PRINT(F("Adjustment X/Y/Z = ")); 408 | DEBUG_PRINT(asax); 409 | DEBUG_PRINT(F(" / ")); 410 | DEBUG_PRINT(asay); 411 | DEBUG_PRINT(F(" / ")); 412 | DEBUG_PRINTLN(asaz); 413 | 414 | DEBUG_PRINTLN(F("Setting magnetometer mode to power-down...")); 415 | //mag -> setMode(0); 416 | I2Cdev::writeByte(0x0E, 0x0A, 0x00); 417 | 418 | // load DMP code into memory banks 419 | DEBUG_PRINT(F("Writing DMP code to MPU memory banks (")); 420 | DEBUG_PRINT(MPU6050_DMP_CODE_SIZE); 421 | DEBUG_PRINTLN(F(" bytes)")); 422 | if (writeProgMemoryBlock(dmpMemory, MPU6050_DMP_CODE_SIZE)) { 423 | DEBUG_PRINTLN(F("Success! DMP code written and verified.")); 424 | 425 | DEBUG_PRINTLN(F("Configuring DMP and related settings...")); 426 | 427 | // write DMP configuration 428 | DEBUG_PRINT(F("Writing DMP configuration to MPU memory banks (")); 429 | DEBUG_PRINT(MPU6050_DMP_CONFIG_SIZE); 430 | DEBUG_PRINTLN(F(" bytes in config def)")); 431 | if (writeProgDMPConfigurationSet(dmpConfig, MPU6050_DMP_CONFIG_SIZE)) { 432 | DEBUG_PRINTLN(F("Success! DMP configuration written and verified.")); 433 | 434 | DEBUG_PRINTLN(F("Setting DMP and FIFO_OFLOW interrupts enabled...")); 435 | setIntEnabled(0x12); 436 | 437 | DEBUG_PRINTLN(F("Setting sample rate to 200Hz...")); 438 | setRate(4); // 1khz / (1 + 4) = 200 Hz 439 | 440 | DEBUG_PRINTLN(F("Setting clock source to Z Gyro...")); 441 | setClockSource(MPU6050_CLOCK_PLL_ZGYRO); 442 | 443 | DEBUG_PRINTLN(F("Setting DLPF bandwidth to 42Hz...")); 444 | setDLPFMode(MPU6050_DLPF_BW_42); 445 | 446 | DEBUG_PRINTLN(F("Setting external frame sync to TEMP_OUT_L[0]...")); 447 | setExternalFrameSync(MPU6050_EXT_SYNC_TEMP_OUT_L); 448 | 449 | DEBUG_PRINTLN(F("Setting gyro sensitivity to +/- 2000 deg/sec...")); 450 | setFullScaleGyroRange(MPU6050_GYRO_FS_2000); 451 | 452 | DEBUG_PRINTLN(F("Setting DMP configuration bytes (function unknown)...")); 453 | setDMPConfig1(0x03); 454 | setDMPConfig2(0x00); 455 | 456 | DEBUG_PRINTLN(F("Clearing OTP Bank flag...")); 457 | setOTPBankValid(false); 458 | 459 | DEBUG_PRINTLN(F("Setting X/Y/Z gyro offsets to previous values...")); 460 | setXGyroOffset(xgOffset); 461 | setYGyroOffset(ygOffset); 462 | setZGyroOffset(zgOffset); 463 | 464 | DEBUG_PRINTLN(F("Setting X/Y/Z gyro user offsets to zero...")); 465 | setXGyroOffsetUser(0); 466 | setYGyroOffsetUser(0); 467 | setZGyroOffsetUser(0); 468 | 469 | DEBUG_PRINTLN(F("Writing final memory update 1/19 (function unknown)...")); 470 | uint8_t dmpUpdate[16], j; 471 | uint16_t pos = 0; 472 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 473 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 474 | 475 | DEBUG_PRINTLN(F("Writing final memory update 2/19 (function unknown)...")); 476 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 477 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 478 | 479 | DEBUG_PRINTLN(F("Resetting FIFO...")); 480 | resetFIFO(); 481 | 482 | DEBUG_PRINTLN(F("Reading FIFO count...")); 483 | uint8_t fifoCount = getFIFOCount(); 484 | 485 | DEBUG_PRINT(F("Current FIFO count=")); 486 | DEBUG_PRINTLN(fifoCount); 487 | uint8_t fifoBuffer[128]; 488 | //getFIFOBytes(fifoBuffer, fifoCount); 489 | 490 | DEBUG_PRINTLN(F("Writing final memory update 3/19 (function unknown)...")); 491 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 492 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 493 | 494 | DEBUG_PRINTLN(F("Writing final memory update 4/19 (function unknown)...")); 495 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 496 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 497 | 498 | DEBUG_PRINTLN(F("Disabling all standby flags...")); 499 | I2Cdev::writeByte(0x68, MPU6050_RA_PWR_MGMT_2, 0x00); 500 | 501 | DEBUG_PRINTLN(F("Setting accelerometer sensitivity to +/- 2g...")); 502 | I2Cdev::writeByte(0x68, MPU6050_RA_ACCEL_CONFIG, 0x00); 503 | 504 | DEBUG_PRINTLN(F("Setting motion detection threshold to 2...")); 505 | setMotionDetectionThreshold(2); 506 | 507 | DEBUG_PRINTLN(F("Setting zero-motion detection threshold to 156...")); 508 | setZeroMotionDetectionThreshold(156); 509 | 510 | DEBUG_PRINTLN(F("Setting motion detection duration to 80...")); 511 | setMotionDetectionDuration(80); 512 | 513 | DEBUG_PRINTLN(F("Setting zero-motion detection duration to 0...")); 514 | setZeroMotionDetectionDuration(0); 515 | 516 | DEBUG_PRINTLN(F("Setting AK8975 to single measurement mode...")); 517 | //mag -> setMode(1); 518 | I2Cdev::writeByte(0x0E, 0x0A, 0x01); 519 | 520 | // setup AK8975 (0x0E) as Slave 0 in read mode 521 | DEBUG_PRINTLN(F("Setting up AK8975 read slave 0...")); 522 | I2Cdev::writeByte(0x68, MPU6050_RA_I2C_SLV0_ADDR, 0x8E); 523 | I2Cdev::writeByte(0x68, MPU6050_RA_I2C_SLV0_REG, 0x01); 524 | I2Cdev::writeByte(0x68, MPU6050_RA_I2C_SLV0_CTRL, 0xDA); 525 | 526 | // setup AK8975 (0x0E) as Slave 2 in write mode 527 | DEBUG_PRINTLN(F("Setting up AK8975 write slave 2...")); 528 | I2Cdev::writeByte(0x68, MPU6050_RA_I2C_SLV2_ADDR, 0x0E); 529 | I2Cdev::writeByte(0x68, MPU6050_RA_I2C_SLV2_REG, 0x0A); 530 | I2Cdev::writeByte(0x68, MPU6050_RA_I2C_SLV2_CTRL, 0x81); 531 | I2Cdev::writeByte(0x68, MPU6050_RA_I2C_SLV2_DO, 0x01); 532 | 533 | // setup I2C timing/delay control 534 | DEBUG_PRINTLN(F("Setting up slave access delay...")); 535 | I2Cdev::writeByte(0x68, MPU6050_RA_I2C_SLV4_CTRL, 0x18); 536 | I2Cdev::writeByte(0x68, MPU6050_RA_I2C_MST_DELAY_CTRL, 0x05); 537 | 538 | // enable interrupts 539 | DEBUG_PRINTLN(F("Enabling default interrupt behavior/no bypass...")); 540 | I2Cdev::writeByte(0x68, MPU6050_RA_INT_PIN_CFG, 0x00); 541 | 542 | // enable I2C master mode and reset DMP/FIFO 543 | DEBUG_PRINTLN(F("Enabling I2C master mode...")); 544 | I2Cdev::writeByte(0x68, MPU6050_RA_USER_CTRL, 0x20); 545 | DEBUG_PRINTLN(F("Resetting FIFO...")); 546 | I2Cdev::writeByte(0x68, MPU6050_RA_USER_CTRL, 0x24); 547 | DEBUG_PRINTLN(F("Rewriting I2C master mode enabled because...I don't know")); 548 | I2Cdev::writeByte(0x68, MPU6050_RA_USER_CTRL, 0x20); 549 | DEBUG_PRINTLN(F("Enabling and resetting DMP/FIFO...")); 550 | I2Cdev::writeByte(0x68, MPU6050_RA_USER_CTRL, 0xE8); 551 | 552 | DEBUG_PRINTLN(F("Writing final memory update 5/19 (function unknown)...")); 553 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 554 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 555 | DEBUG_PRINTLN(F("Writing final memory update 6/19 (function unknown)...")); 556 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 557 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 558 | DEBUG_PRINTLN(F("Writing final memory update 7/19 (function unknown)...")); 559 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 560 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 561 | DEBUG_PRINTLN(F("Writing final memory update 8/19 (function unknown)...")); 562 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 563 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 564 | DEBUG_PRINTLN(F("Writing final memory update 9/19 (function unknown)...")); 565 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 566 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 567 | DEBUG_PRINTLN(F("Writing final memory update 10/19 (function unknown)...")); 568 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 569 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 570 | DEBUG_PRINTLN(F("Writing final memory update 11/19 (function unknown)...")); 571 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 572 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 573 | 574 | DEBUG_PRINTLN(F("Reading final memory update 12/19 (function unknown)...")); 575 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 576 | readMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 577 | #ifdef DEBUG 578 | DEBUG_PRINT(F("Read bytes: ")); 579 | for (j = 0; j < 4; j++) { 580 | DEBUG_PRINTF(dmpUpdate[3 + j], HEX); 581 | DEBUG_PRINT(" "); 582 | } 583 | DEBUG_PRINTLN(""); 584 | #endif 585 | 586 | DEBUG_PRINTLN(F("Writing final memory update 13/19 (function unknown)...")); 587 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 588 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 589 | DEBUG_PRINTLN(F("Writing final memory update 14/19 (function unknown)...")); 590 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 591 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 592 | DEBUG_PRINTLN(F("Writing final memory update 15/19 (function unknown)...")); 593 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 594 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 595 | DEBUG_PRINTLN(F("Writing final memory update 16/19 (function unknown)...")); 596 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 597 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 598 | DEBUG_PRINTLN(F("Writing final memory update 17/19 (function unknown)...")); 599 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 600 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 601 | 602 | DEBUG_PRINTLN(F("Waiting for FIRO count >= 46...")); 603 | while ((fifoCount = getFIFOCount()) < 46); 604 | DEBUG_PRINTLN(F("Reading FIFO...")); 605 | getFIFOBytes(fifoBuffer, min(fifoCount, 128)); // safeguard only 128 bytes 606 | DEBUG_PRINTLN(F("Reading interrupt status...")); 607 | getIntStatus(); 608 | 609 | DEBUG_PRINTLN(F("Writing final memory update 18/19 (function unknown)...")); 610 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 611 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 612 | 613 | DEBUG_PRINTLN(F("Waiting for FIRO count >= 48...")); 614 | while ((fifoCount = getFIFOCount()) < 48); 615 | DEBUG_PRINTLN(F("Reading FIFO...")); 616 | getFIFOBytes(fifoBuffer, min(fifoCount, 128)); // safeguard only 128 bytes 617 | DEBUG_PRINTLN(F("Reading interrupt status...")); 618 | getIntStatus(); 619 | DEBUG_PRINTLN(F("Waiting for FIRO count >= 48...")); 620 | while ((fifoCount = getFIFOCount()) < 48); 621 | DEBUG_PRINTLN(F("Reading FIFO...")); 622 | getFIFOBytes(fifoBuffer, min(fifoCount, 128)); // safeguard only 128 bytes 623 | DEBUG_PRINTLN(F("Reading interrupt status...")); 624 | getIntStatus(); 625 | 626 | DEBUG_PRINTLN(F("Writing final memory update 19/19 (function unknown)...")); 627 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 628 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 629 | 630 | DEBUG_PRINTLN(F("Disabling DMP (you turn it on later)...")); 631 | setDMPEnabled(false); 632 | 633 | DEBUG_PRINTLN(F("Setting up internal 48-byte (default) DMP packet buffer...")); 634 | dmpPacketSize = 48; 635 | /*if ((dmpPacketBuffer = (uint8_t *)malloc(42)) == 0) { 636 | return 3; // TODO: proper error code for no memory 637 | }*/ 638 | 639 | DEBUG_PRINTLN(F("Resetting FIFO and clearing INT status one last time...")); 640 | resetFIFO(); 641 | getIntStatus(); 642 | } else { 643 | DEBUG_PRINTLN(F("ERROR! DMP configuration verification failed.")); 644 | return 2; // configuration block loading failed 645 | } 646 | } else { 647 | DEBUG_PRINTLN(F("ERROR! DMP code verification failed.")); 648 | return 1; // main binary block loading failed 649 | } 650 | return 0; // success 651 | } 652 | 653 | bool MPU6050::dmpPacketAvailable() { 654 | return getFIFOCount() >= dmpGetFIFOPacketSize(); 655 | } 656 | 657 | // uint8_t MPU6050::dmpSetFIFORate(uint8_t fifoRate); 658 | // uint8_t MPU6050::dmpGetFIFORate(); 659 | // uint8_t MPU6050::dmpGetSampleStepSizeMS(); 660 | // uint8_t MPU6050::dmpGetSampleFrequency(); 661 | // int32_t MPU6050::dmpDecodeTemperature(int8_t tempReg); 662 | 663 | //uint8_t MPU6050::dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority); 664 | //uint8_t MPU6050::dmpUnregisterFIFORateProcess(inv_obj_func func); 665 | //uint8_t MPU6050::dmpRunFIFORateProcesses(); 666 | 667 | // uint8_t MPU6050::dmpSendQuaternion(uint_fast16_t accuracy); 668 | // uint8_t MPU6050::dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy); 669 | // uint8_t MPU6050::dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy); 670 | // uint8_t MPU6050::dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy); 671 | // uint8_t MPU6050::dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy); 672 | // uint8_t MPU6050::dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy); 673 | // uint8_t MPU6050::dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy); 674 | // uint8_t MPU6050::dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy); 675 | // uint8_t MPU6050::dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy); 676 | // uint8_t MPU6050::dmpSendPacketNumber(uint_fast16_t accuracy); 677 | // uint8_t MPU6050::dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy); 678 | // uint8_t MPU6050::dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy); 679 | 680 | uint8_t MPU6050::dmpGetAccel(int32_t *data, const uint8_t* packet) { 681 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) 682 | if (packet == 0) packet = dmpPacketBuffer; 683 | data[0] = ((packet[34] << 24) + (packet[35] << 16) + (packet[36] << 8) + packet[37]); 684 | data[1] = ((packet[38] << 24) + (packet[39] << 16) + (packet[40] << 8) + packet[41]); 685 | data[2] = ((packet[42] << 24) + (packet[43] << 16) + (packet[44] << 8) + packet[45]); 686 | return 0; 687 | } 688 | uint8_t MPU6050::dmpGetAccel(int16_t *data, const uint8_t* packet) { 689 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) 690 | if (packet == 0) packet = dmpPacketBuffer; 691 | data[0] = (packet[34] << 8) + packet[35]; 692 | data[1] = (packet[38] << 8) + packet[39]; 693 | data[2] = (packet[42] << 8) + packet[43]; 694 | return 0; 695 | } 696 | uint8_t MPU6050::dmpGetAccel(VectorInt16 *v, const uint8_t* packet) { 697 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) 698 | if (packet == 0) packet = dmpPacketBuffer; 699 | v -> x = (packet[34] << 8) + packet[35]; 700 | v -> y = (packet[38] << 8) + packet[39]; 701 | v -> z = (packet[42] << 8) + packet[43]; 702 | return 0; 703 | } 704 | uint8_t MPU6050::dmpGetQuaternion(int32_t *data, const uint8_t* packet) { 705 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) 706 | if (packet == 0) packet = dmpPacketBuffer; 707 | data[0] = ((packet[0] << 24) + (packet[1] << 16) + (packet[2] << 8) + packet[3]); 708 | data[1] = ((packet[4] << 24) + (packet[5] << 16) + (packet[6] << 8) + packet[7]); 709 | data[2] = ((packet[8] << 24) + (packet[9] << 16) + (packet[10] << 8) + packet[11]); 710 | data[3] = ((packet[12] << 24) + (packet[13] << 16) + (packet[14] << 8) + packet[15]); 711 | return 0; 712 | } 713 | uint8_t MPU6050::dmpGetQuaternion(int16_t *data, const uint8_t* packet) { 714 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) 715 | if (packet == 0) packet = dmpPacketBuffer; 716 | data[0] = ((packet[0] << 8) + packet[1]); 717 | data[1] = ((packet[4] << 8) + packet[5]); 718 | data[2] = ((packet[8] << 8) + packet[9]); 719 | data[3] = ((packet[12] << 8) + packet[13]); 720 | return 0; 721 | } 722 | uint8_t MPU6050::dmpGetQuaternion(Quaternion *q, const uint8_t* packet) { 723 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) 724 | int16_t qI[4]; 725 | uint8_t status = dmpGetQuaternion(qI, packet); 726 | if (status == 0) { 727 | q -> w = (float)qI[0] / 16384.0f; 728 | q -> x = (float)qI[1] / 16384.0f; 729 | q -> y = (float)qI[2] / 16384.0f; 730 | q -> z = (float)qI[3] / 16384.0f; 731 | return 0; 732 | } 733 | return status; // int16 return value, indicates error if this line is reached 734 | } 735 | // uint8_t MPU6050::dmpGet6AxisQuaternion(long *data, const uint8_t* packet); 736 | // uint8_t MPU6050::dmpGetRelativeQuaternion(long *data, const uint8_t* packet); 737 | uint8_t MPU6050::dmpGetGyro(int32_t *data, const uint8_t* packet) { 738 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) 739 | if (packet == 0) packet = dmpPacketBuffer; 740 | data[0] = ((packet[16] << 24) + (packet[17] << 16) + (packet[18] << 8) + packet[19]); 741 | data[1] = ((packet[20] << 24) + (packet[21] << 16) + (packet[22] << 8) + packet[23]); 742 | data[2] = ((packet[24] << 24) + (packet[25] << 16) + (packet[26] << 8) + packet[27]); 743 | return 0; 744 | } 745 | uint8_t MPU6050::dmpGetGyro(int16_t *data, const uint8_t* packet) { 746 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) 747 | if (packet == 0) packet = dmpPacketBuffer; 748 | data[0] = (packet[16] << 8) + packet[17]; 749 | data[1] = (packet[20] << 8) + packet[21]; 750 | data[2] = (packet[24] << 8) + packet[25]; 751 | return 0; 752 | } 753 | uint8_t MPU6050::dmpGetMag(int16_t *data, const uint8_t* packet) { 754 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) 755 | if (packet == 0) packet = dmpPacketBuffer; 756 | data[0] = (packet[28] << 8) + packet[29]; 757 | data[1] = (packet[30] << 8) + packet[31]; 758 | data[2] = (packet[32] << 8) + packet[33]; 759 | return 0; 760 | } 761 | // uint8_t MPU6050::dmpSetLinearAccelFilterCoefficient(float coef); 762 | // uint8_t MPU6050::dmpGetLinearAccel(long *data, const uint8_t* packet); 763 | uint8_t MPU6050::dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity) { 764 | // get rid of the gravity component (+1g = +4096 in standard DMP FIFO packet) 765 | v -> x = vRaw -> x - gravity -> x*4096; 766 | v -> y = vRaw -> y - gravity -> y*4096; 767 | v -> z = vRaw -> z - gravity -> z*4096; 768 | return 0; 769 | } 770 | // uint8_t MPU6050::dmpGetLinearAccelInWorld(long *data, const uint8_t* packet); 771 | uint8_t MPU6050::dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q) { 772 | // rotate measured 3D acceleration vector into original state 773 | // frame of reference based on orientation quaternion 774 | memcpy(v, vReal, sizeof(VectorInt16)); 775 | v -> rotate(q); 776 | return 0; 777 | } 778 | // uint8_t MPU6050::dmpGetGyroAndAccelSensor(long *data, const uint8_t* packet); 779 | // uint8_t MPU6050::dmpGetGyroSensor(long *data, const uint8_t* packet); 780 | // uint8_t MPU6050::dmpGetControlData(long *data, const uint8_t* packet); 781 | // uint8_t MPU6050::dmpGetTemperature(long *data, const uint8_t* packet); 782 | // uint8_t MPU6050::dmpGetGravity(long *data, const uint8_t* packet); 783 | uint8_t MPU6050::dmpGetGravity(VectorFloat *v, Quaternion *q) { 784 | v -> x = 2 * (q -> x*q -> z - q -> w*q -> y); 785 | v -> y = 2 * (q -> w*q -> x + q -> y*q -> z); 786 | v -> z = q -> w*q -> w - q -> x*q -> x - q -> y*q -> y + q -> z*q -> z; 787 | return 0; 788 | } 789 | // uint8_t MPU6050::dmpGetUnquantizedAccel(long *data, const uint8_t* packet); 790 | // uint8_t MPU6050::dmpGetQuantizedAccel(long *data, const uint8_t* packet); 791 | // uint8_t MPU6050::dmpGetExternalSensorData(long *data, int size, const uint8_t* packet); 792 | // uint8_t MPU6050::dmpGetEIS(long *data, const uint8_t* packet); 793 | 794 | uint8_t MPU6050::dmpGetEuler(float *data, Quaternion *q) { 795 | 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 796 | data[1] = -asin(2*q -> x*q -> z + 2*q -> w*q -> y); // theta 797 | 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 798 | return 0; 799 | } 800 | uint8_t MPU6050::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) { 801 | // yaw: (about Z axis) 802 | data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1); 803 | // pitch: (nose up/down, about Y axis) 804 | data[1] = atan(gravity -> x / sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z)); 805 | // roll: (tilt left/right, about X axis) 806 | data[2] = atan(gravity -> y / sqrt(gravity -> x*gravity -> x + gravity -> z*gravity -> z)); 807 | return 0; 808 | } 809 | 810 | // uint8_t MPU6050::dmpGetAccelFloat(float *data, const uint8_t* packet); 811 | // uint8_t MPU6050::dmpGetQuaternionFloat(float *data, const uint8_t* packet); 812 | 813 | uint8_t MPU6050::dmpProcessFIFOPacket(const unsigned char *dmpData) { 814 | /*for (uint8_t k = 0; k < dmpPacketSize; k++) { 815 | if (dmpData[k] < 0x10) Serial.print("0"); 816 | Serial.print(dmpData[k], HEX); 817 | Serial.print(" "); 818 | } 819 | Serial.print("\n");*/ 820 | //Serial.println((uint16_t)dmpPacketBuffer); 821 | return 0; 822 | } 823 | uint8_t MPU6050::dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed) { 824 | uint8_t status; 825 | uint8_t buf[dmpPacketSize]; 826 | for (uint8_t i = 0; i < numPackets; i++) { 827 | // read packet from FIFO 828 | getFIFOBytes(buf, dmpPacketSize); 829 | 830 | // process packet 831 | if ((status = dmpProcessFIFOPacket(buf)) > 0) return status; 832 | 833 | // increment external process count variable, if supplied 834 | if (processed != 0) *processed++; 835 | } 836 | return 0; 837 | } 838 | 839 | // uint8_t MPU6050::dmpSetFIFOProcessedCallback(void (*func) (void)); 840 | 841 | // uint8_t MPU6050::dmpInitFIFOParam(); 842 | // uint8_t MPU6050::dmpCloseFIFO(); 843 | // uint8_t MPU6050::dmpSetGyroDataSource(uint_fast8_t source); 844 | // uint8_t MPU6050::dmpDecodeQuantizedAccel(); 845 | // uint32_t MPU6050::dmpGetGyroSumOfSquare(); 846 | // uint32_t MPU6050::dmpGetAccelSumOfSquare(); 847 | // void MPU6050::dmpOverrideQuaternion(long *q); 848 | uint16_t MPU6050::dmpGetFIFOPacketSize() { 849 | return dmpPacketSize; 850 | } 851 | 852 | #endif /* _MPU6050_9AXIS_MOTIONAPPS41_H_ */ 853 | --------------------------------------------------------------------------------