├── Franko └── Franko.ino ├── README.md └── libraries ├── LMotorController ├── LMotorController.cpp └── LMotorController.h ├── MPU6050 ├── MPU6050.cpp ├── MPU6050.h ├── MPU6050_6Axis_MotionApps20.h ├── MPU6050_9Axis_MotionApps41.h └── helper_3dmath.h └── PID_v1 ├── PID_v1.cpp └── PID_v1.h /Franko/Franko.ino: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include "I2Cdev.h" 4 | 5 | #include "MPU6050_6Axis_MotionApps20.h" 6 | 7 | #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE 8 | #include "Wire.h" 9 | #endif 10 | 11 | 12 | #define LOG_INPUT 0 13 | #define MANUAL_TUNING 0 14 | #define LOG_PID_CONSTANTS 0 //MANUAL_TUNING must be 1 15 | #define MOVE_BACK_FORTH 0 16 | 17 | #define MIN_ABS_SPEED 30 18 | 19 | //MPU 20 | 21 | 22 | MPU6050 mpu; 23 | 24 | // MPU control/status vars 25 | bool dmpReady = false; // set true if DMP init was successful 26 | uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU 27 | uint8_t devStatus; // return status after each device operation (0 = success, !0 = error) 28 | uint16_t packetSize; // expected DMP packet size (default is 42 bytes) 29 | uint16_t fifoCount; // count of all bytes currently in FIFO 30 | uint8_t fifoBuffer[64]; // FIFO storage buffer 31 | 32 | // orientation/motion vars 33 | Quaternion q; // [w, x, y, z] quaternion container 34 | VectorFloat gravity; // [x, y, z] gravity vector 35 | float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector 36 | 37 | 38 | //PID 39 | 40 | 41 | #if MANUAL_TUNING 42 | double kp , ki, kd; 43 | double prevKp, prevKi, prevKd; 44 | #endif 45 | double originalSetpoint = 174.29; 46 | double setpoint = originalSetpoint; 47 | double movingAngleOffset = 0.3; 48 | double input, output; 49 | int moveState=0; //0 = balance; 1 = back; 2 = forth 50 | 51 | #if MANUAL_TUNING 52 | PID pid(&input, &output, &setpoint, 0, 0, 0, DIRECT); 53 | #else 54 | PID pid(&input, &output, &setpoint, 70, 240, 1.9, DIRECT); 55 | #endif 56 | 57 | 58 | //MOTOR CONTROLLER 59 | 60 | 61 | int ENA = 3; 62 | int IN1 = 4; 63 | int IN2 = 8; 64 | int IN3 = 5; 65 | int IN4 = 7; 66 | int ENB = 6; 67 | 68 | 69 | LMotorController motorController(ENA, IN1, IN2, ENB, IN3, IN4, 0.6, 1); 70 | 71 | 72 | //timers 73 | 74 | 75 | long time1Hz = 0; 76 | long time5Hz = 0; 77 | 78 | 79 | volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high 80 | void dmpDataReady() 81 | { 82 | mpuInterrupt = true; 83 | } 84 | 85 | 86 | void setup() 87 | { 88 | // join I2C bus (I2Cdev library doesn't do this automatically) 89 | #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE 90 | Wire.begin(); 91 | TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz) 92 | #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE 93 | Fastwire::setup(400, true); 94 | #endif 95 | 96 | // initialize serial communication 97 | // (115200 chosen because it is required for Teapot Demo output, but it's 98 | // really up to you depending on your project) 99 | Serial.begin(115200); 100 | while (!Serial); // wait for Leonardo enumeration, others continue immediately 101 | 102 | // initialize device 103 | Serial.println(F("Initializing I2C devices...")); 104 | mpu.initialize(); 105 | 106 | // verify connection 107 | Serial.println(F("Testing device connections...")); 108 | Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed")); 109 | 110 | // load and configure the DMP 111 | Serial.println(F("Initializing DMP...")); 112 | devStatus = mpu.dmpInitialize(); 113 | 114 | // supply your own gyro offsets here, scaled for min sensitivity 115 | mpu.setXGyroOffset(220); 116 | mpu.setYGyroOffset(76); 117 | mpu.setZGyroOffset(-85); 118 | mpu.setZAccelOffset(1788); // 1688 factory default for my test chip 119 | 120 | // make sure it worked (returns 0 if so) 121 | if (devStatus == 0) 122 | { 123 | // turn on the DMP, now that it's ready 124 | Serial.println(F("Enabling DMP...")); 125 | mpu.setDMPEnabled(true); 126 | 127 | // enable Arduino interrupt detection 128 | Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)...")); 129 | attachInterrupt(0, dmpDataReady, RISING); 130 | mpuIntStatus = mpu.getIntStatus(); 131 | 132 | // set our DMP Ready flag so the main loop() function knows it's okay to use it 133 | Serial.println(F("DMP ready! Waiting for first interrupt...")); 134 | dmpReady = true; 135 | 136 | // get expected DMP packet size for later comparison 137 | packetSize = mpu.dmpGetFIFOPacketSize(); 138 | 139 | //setup PID 140 | 141 | pid.SetMode(AUTOMATIC); 142 | pid.SetSampleTime(10); 143 | pid.SetOutputLimits(-255, 255); 144 | } 145 | else 146 | { 147 | // ERROR! 148 | // 1 = initial memory load failed 149 | // 2 = DMP configuration updates failed 150 | // (if it's going to break, usually the code will be 1) 151 | Serial.print(F("DMP Initialization failed (code ")); 152 | Serial.print(devStatus); 153 | Serial.println(F(")")); 154 | } 155 | } 156 | 157 | 158 | void loop() 159 | { 160 | // if programming failed, don't try to do anything 161 | if (!dmpReady) return; 162 | 163 | // wait for MPU interrupt or extra packet(s) available 164 | while (!mpuInterrupt && fifoCount < packetSize) 165 | { 166 | //no mpu data - performing PID calculations and output to motors 167 | 168 | pid.Compute(); 169 | motorController.move(output, MIN_ABS_SPEED); 170 | 171 | unsigned long currentMillis = millis(); 172 | 173 | if (currentMillis - time1Hz >= 1000) 174 | { 175 | loopAt1Hz(); 176 | time1Hz = currentMillis; 177 | } 178 | 179 | if (currentMillis - time5Hz >= 5000) 180 | { 181 | loopAt5Hz(); 182 | time5Hz = currentMillis; 183 | } 184 | } 185 | 186 | // reset interrupt flag and get INT_STATUS byte 187 | mpuInterrupt = false; 188 | mpuIntStatus = mpu.getIntStatus(); 189 | 190 | // get current FIFO count 191 | fifoCount = mpu.getFIFOCount(); 192 | 193 | // check for overflow (this should never happen unless our code is too inefficient) 194 | if ((mpuIntStatus & 0x10) || fifoCount == 1024) 195 | { 196 | // reset so we can continue cleanly 197 | mpu.resetFIFO(); 198 | Serial.println(F("FIFO overflow!")); 199 | 200 | // otherwise, check for DMP data ready interrupt (this should happen frequently) 201 | } 202 | else if (mpuIntStatus & 0x02) 203 | { 204 | // wait for correct available data length, should be a VERY short wait 205 | while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount(); 206 | 207 | // read a packet from FIFO 208 | mpu.getFIFOBytes(fifoBuffer, packetSize); 209 | 210 | // track FIFO count here in case there is > 1 packet available 211 | // (this lets us immediately read more without waiting for an interrupt) 212 | fifoCount -= packetSize; 213 | 214 | mpu.dmpGetQuaternion(&q, fifoBuffer); 215 | mpu.dmpGetGravity(&gravity, &q); 216 | mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); 217 | #if LOG_INPUT 218 | Serial.print("ypr\t"); 219 | Serial.print(ypr[0] * 180/M_PI); 220 | Serial.print("\t"); 221 | Serial.print(ypr[1] * 180/M_PI); 222 | Serial.print("\t"); 223 | Serial.println(ypr[2] * 180/M_PI); 224 | #endif 225 | input = ypr[1] * 180/M_PI + 180; 226 | } 227 | } 228 | 229 | 230 | void loopAt1Hz() 231 | { 232 | #if MANUAL_TUNING 233 | setPIDTuningValues(); 234 | #endif 235 | } 236 | 237 | 238 | void loopAt5Hz() 239 | { 240 | #if MOVE_BACK_FORTH 241 | moveBackForth(); 242 | #endif 243 | } 244 | 245 | 246 | //move back and forth 247 | 248 | 249 | void moveBackForth() 250 | { 251 | moveState++; 252 | if (moveState > 2) moveState = 0; 253 | 254 | if (moveState == 0) 255 | setpoint = originalSetpoint; 256 | else if (moveState == 1) 257 | setpoint = originalSetpoint - movingAngleOffset; 258 | else 259 | setpoint = originalSetpoint + movingAngleOffset; 260 | } 261 | 262 | 263 | //PID Tuning (3 potentiometers) 264 | 265 | #if MANUAL_TUNING 266 | void setPIDTuningValues() 267 | { 268 | readPIDTuningValues(); 269 | 270 | if (kp != prevKp || ki != prevKi || kd != prevKd) 271 | { 272 | #if LOG_PID_CONSTANTS 273 | Serial.print(kp);Serial.print(", ");Serial.print(ki);Serial.print(", ");Serial.println(kd); 274 | #endif 275 | 276 | pid.SetTunings(kp, ki, kd); 277 | prevKp = kp; prevKi = ki; prevKd = kd; 278 | } 279 | } 280 | 281 | 282 | void readPIDTuningValues() 283 | { 284 | int potKp = analogRead(A0); 285 | int potKi = analogRead(A1); 286 | int potKd = analogRead(A2); 287 | 288 | kp = map(potKp, 0, 1023, 0, 25000) / 100.0; //0 - 250 289 | ki = map(potKi, 0, 1023, 0, 100000) / 100.0; //0 - 1000 290 | kd = map(potKd, 0, 1023, 0, 500) / 100.0; //0 - 5 291 | } 292 | #endif 293 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | Franko 2 | ====== 3 | 4 | Arduino Self balancing robot 5 | 6 | Repo structure 7 | -------------- 8 | * Franko/Franko.ino: Arduino sketch 9 | * Libraries/*: Libraries should be copied to local Arduino libraries folder 10 | 11 | Components 12 | ---------- 13 | * Arduino UNO 14 | * 6 DOF MPU6050 (GY521) (Accel + Gyro) 15 | * L298N Motor Driver 16 | * 2 x 12V 122rpm DC motors + 80mm wheels 17 | * 3 potentiometers for PID tunings 18 | 19 | Photo 20 | ----- 21 | 22 | [![](https://dl.dropboxusercontent.com/u/18883987/Franko.jpg)](https://dl.dropboxusercontent.com/u/18883987/Franko.jpg) 23 | -------------------------------------------------------------------------------- /libraries/LMotorController/LMotorController.cpp: -------------------------------------------------------------------------------- 1 | #include "LMotorController.h" 2 | #include "Arduino.h" 3 | 4 | 5 | LMotorController::LMotorController(int ena, int in1, int in2, int enb, int in3, int in4, double motorAConst, double motorBConst) 6 | { 7 | _motorAConst = motorAConst; 8 | _motorBConst = motorBConst; 9 | 10 | _ena = ena; 11 | _in1 = in1; 12 | _in2 = in2; 13 | _enb = enb; 14 | _in3 = in3; 15 | _in4 = in4; 16 | 17 | pinMode(_ena, OUTPUT); 18 | pinMode(_in1, OUTPUT); 19 | pinMode(_in2, OUTPUT); 20 | 21 | pinMode(_enb, OUTPUT); 22 | pinMode(_in3, OUTPUT); 23 | pinMode(_in4, OUTPUT); 24 | } 25 | 26 | 27 | void LMotorController::move(int leftSpeed, int rightSpeed, int minAbsSpeed) 28 | { 29 | if (rightSpeed < 0) 30 | { 31 | rightSpeed = min(rightSpeed, -1*minAbsSpeed); 32 | rightSpeed = max(rightSpeed, -255); 33 | } 34 | else if (rightSpeed > 0) 35 | { 36 | rightSpeed = max(rightSpeed, minAbsSpeed); 37 | rightSpeed = min(rightSpeed, 255); 38 | } 39 | 40 | int realRightSpeed = map(abs(rightSpeed), 0, 255, minAbsSpeed, 255); 41 | 42 | if (leftSpeed < 0) 43 | { 44 | leftSpeed = min(leftSpeed, -1*minAbsSpeed); 45 | leftSpeed = max(leftSpeed, -255); 46 | } 47 | else if (leftSpeed > 0) 48 | { 49 | leftSpeed = max(leftSpeed, minAbsSpeed); 50 | leftSpeed = min(leftSpeed, 255); 51 | } 52 | 53 | int realLeftSpeed = map(abs(leftSpeed), 0, 255, minAbsSpeed, 255); 54 | 55 | digitalWrite(_in3, rightSpeed > 0 ? HIGH : LOW); 56 | digitalWrite(_in4, rightSpeed > 0 ? LOW : HIGH); 57 | digitalWrite(_in1, leftSpeed > 0 ? HIGH : LOW); 58 | digitalWrite(_in2, leftSpeed > 0 ? LOW : HIGH); 59 | analogWrite(_ena, realRightSpeed * _motorAConst); 60 | analogWrite(_enb, realLeftSpeed * _motorBConst); 61 | } 62 | 63 | 64 | void LMotorController::move(int speed, int minAbsSpeed) 65 | { 66 | int direction = 1; 67 | 68 | if (speed < 0) 69 | { 70 | direction = -1; 71 | 72 | speed = min(speed, -1*minAbsSpeed); 73 | speed = max(speed, -255); 74 | } 75 | else 76 | { 77 | speed = max(speed, minAbsSpeed); 78 | speed = min(speed, 255); 79 | } 80 | 81 | if (speed == _currentSpeed) return; 82 | 83 | int realSpeed = max(minAbsSpeed, abs(speed)); 84 | 85 | digitalWrite(_in1, speed > 0 ? HIGH : LOW); 86 | digitalWrite(_in2, speed > 0 ? LOW : HIGH); 87 | digitalWrite(_in3, speed > 0 ? HIGH : LOW); 88 | digitalWrite(_in4, speed > 0 ? LOW : HIGH); 89 | analogWrite(_ena, realSpeed * _motorAConst); 90 | analogWrite(_enb, realSpeed * _motorBConst); 91 | 92 | _currentSpeed = direction * realSpeed; 93 | } 94 | 95 | 96 | void LMotorController::move(int speed) 97 | { 98 | if (speed == _currentSpeed) return; 99 | 100 | if (speed > 255) speed = 255; 101 | else if (speed < -255) speed = -255; 102 | 103 | digitalWrite(_in1, speed > 0 ? HIGH : LOW); 104 | digitalWrite(_in2, speed > 0 ? LOW : HIGH); 105 | digitalWrite(_in3, speed > 0 ? HIGH : LOW); 106 | digitalWrite(_in4, speed > 0 ? LOW : HIGH); 107 | analogWrite(_ena, abs(speed) * _motorAConst); 108 | analogWrite(_enb, abs(speed) * _motorBConst); 109 | 110 | _currentSpeed = speed; 111 | } 112 | 113 | 114 | void LMotorController::turnLeft(int speed, bool kick) 115 | { 116 | digitalWrite(_in1, HIGH); 117 | digitalWrite(_in2, LOW); 118 | digitalWrite(_in3, LOW); 119 | digitalWrite(_in4, HIGH); 120 | 121 | if (kick) 122 | { 123 | analogWrite(_ena, 255); 124 | analogWrite(_enb, 255); 125 | 126 | delay(100); 127 | } 128 | 129 | analogWrite(_ena, speed * _motorAConst); 130 | analogWrite(_enb, speed * _motorBConst); 131 | } 132 | 133 | 134 | void LMotorController::turnRight(int speed, bool kick) 135 | { 136 | digitalWrite(_in1, LOW); 137 | digitalWrite(_in2, HIGH); 138 | digitalWrite(_in3, HIGH); 139 | digitalWrite(_in4, LOW); 140 | 141 | if (kick) 142 | { 143 | analogWrite(_ena, 255); 144 | analogWrite(_enb, 255); 145 | 146 | delay(100); 147 | } 148 | 149 | analogWrite(_ena, speed * _motorAConst); 150 | analogWrite(_enb, speed * _motorBConst); 151 | } 152 | 153 | 154 | void LMotorController::stopMoving() 155 | { 156 | digitalWrite(_in1, LOW); 157 | digitalWrite(_in2, LOW); 158 | digitalWrite(_in3, LOW); 159 | digitalWrite(_in4, LOW); 160 | digitalWrite(_ena, HIGH); 161 | digitalWrite(_enb, HIGH); 162 | 163 | _currentSpeed = 0; 164 | } -------------------------------------------------------------------------------- /libraries/LMotorController/LMotorController.h: -------------------------------------------------------------------------------- 1 | #ifndef LMotorController_h 2 | #define LMotorController_h 3 | 4 | 5 | #include "Arduino.h" 6 | 7 | 8 | class LMotorController 9 | { 10 | protected: 11 | int _ena, _in1, _in2, _enb, _in3, _in4; 12 | int _currentSpeed; 13 | double _motorAConst, _motorBConst; 14 | public: 15 | LMotorController(int ena, int in1, int in2, int enb, int in3, int in4, double motorAConst, double motorBConst); 16 | void move(int leftSpeed, int rightSpeed, int minAbsSpeed); 17 | void move(int speed); 18 | void move(int speed, int minAbsSpeed); 19 | void turnLeft(int speed, bool kick); 20 | void turnRight(int speed, bool kick); 21 | void stopMoving(); 22 | }; 23 | 24 | 25 | #endif -------------------------------------------------------------------------------- /libraries/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 | 42 | // supporting link: http://forum.arduino.cc/index.php?&topic=143444.msg1079517#msg1079517 43 | // also: http://forum.arduino.cc/index.php?&topic=141571.msg1062899#msg1062899s 44 | #ifndef __arm__ 45 | #include 46 | #else 47 | #define PROGMEM /* empty */ 48 | #define pgm_read_byte(x) (*(x)) 49 | #define pgm_read_word(x) (*(x)) 50 | #define pgm_read_float(x) (*(x)) 51 | #define PSTR(STR) STR 52 | #endif 53 | 54 | 55 | #define MPU6050_ADDRESS_AD0_LOW 0x68 // address pin low (GND), default for InvenSense evaluation board 56 | #define MPU6050_ADDRESS_AD0_HIGH 0x69 // address pin high (VCC) 57 | #define MPU6050_DEFAULT_ADDRESS MPU6050_ADDRESS_AD0_LOW 58 | 59 | #define MPU6050_RA_XG_OFFS_TC 0x00 //[7] PWR_MODE, [6:1] XG_OFFS_TC, [0] OTP_BNK_VLD 60 | #define MPU6050_RA_YG_OFFS_TC 0x01 //[7] PWR_MODE, [6:1] YG_OFFS_TC, [0] OTP_BNK_VLD 61 | #define MPU6050_RA_ZG_OFFS_TC 0x02 //[7] PWR_MODE, [6:1] ZG_OFFS_TC, [0] OTP_BNK_VLD 62 | #define MPU6050_RA_X_FINE_GAIN 0x03 //[7:0] X_FINE_GAIN 63 | #define MPU6050_RA_Y_FINE_GAIN 0x04 //[7:0] Y_FINE_GAIN 64 | #define MPU6050_RA_Z_FINE_GAIN 0x05 //[7:0] Z_FINE_GAIN 65 | #define MPU6050_RA_XA_OFFS_H 0x06 //[15:0] XA_OFFS 66 | #define MPU6050_RA_XA_OFFS_L_TC 0x07 67 | #define MPU6050_RA_YA_OFFS_H 0x08 //[15:0] YA_OFFS 68 | #define MPU6050_RA_YA_OFFS_L_TC 0x09 69 | #define MPU6050_RA_ZA_OFFS_H 0x0A //[15:0] ZA_OFFS 70 | #define MPU6050_RA_ZA_OFFS_L_TC 0x0B 71 | #define MPU6050_RA_XG_OFFS_USRH 0x13 //[15:0] XG_OFFS_USR 72 | #define MPU6050_RA_XG_OFFS_USRL 0x14 73 | #define MPU6050_RA_YG_OFFS_USRH 0x15 //[15:0] YG_OFFS_USR 74 | #define MPU6050_RA_YG_OFFS_USRL 0x16 75 | #define MPU6050_RA_ZG_OFFS_USRH 0x17 //[15:0] ZG_OFFS_USR 76 | #define MPU6050_RA_ZG_OFFS_USRL 0x18 77 | #define MPU6050_RA_SMPLRT_DIV 0x19 78 | #define MPU6050_RA_CONFIG 0x1A 79 | #define MPU6050_RA_GYRO_CONFIG 0x1B 80 | #define MPU6050_RA_ACCEL_CONFIG 0x1C 81 | #define MPU6050_RA_FF_THR 0x1D 82 | #define MPU6050_RA_FF_DUR 0x1E 83 | #define MPU6050_RA_MOT_THR 0x1F 84 | #define MPU6050_RA_MOT_DUR 0x20 85 | #define MPU6050_RA_ZRMOT_THR 0x21 86 | #define MPU6050_RA_ZRMOT_DUR 0x22 87 | #define MPU6050_RA_FIFO_EN 0x23 88 | #define MPU6050_RA_I2C_MST_CTRL 0x24 89 | #define MPU6050_RA_I2C_SLV0_ADDR 0x25 90 | #define MPU6050_RA_I2C_SLV0_REG 0x26 91 | #define MPU6050_RA_I2C_SLV0_CTRL 0x27 92 | #define MPU6050_RA_I2C_SLV1_ADDR 0x28 93 | #define MPU6050_RA_I2C_SLV1_REG 0x29 94 | #define MPU6050_RA_I2C_SLV1_CTRL 0x2A 95 | #define MPU6050_RA_I2C_SLV2_ADDR 0x2B 96 | #define MPU6050_RA_I2C_SLV2_REG 0x2C 97 | #define MPU6050_RA_I2C_SLV2_CTRL 0x2D 98 | #define MPU6050_RA_I2C_SLV3_ADDR 0x2E 99 | #define MPU6050_RA_I2C_SLV3_REG 0x2F 100 | #define MPU6050_RA_I2C_SLV3_CTRL 0x30 101 | #define MPU6050_RA_I2C_SLV4_ADDR 0x31 102 | #define MPU6050_RA_I2C_SLV4_REG 0x32 103 | #define MPU6050_RA_I2C_SLV4_DO 0x33 104 | #define MPU6050_RA_I2C_SLV4_CTRL 0x34 105 | #define MPU6050_RA_I2C_SLV4_DI 0x35 106 | #define MPU6050_RA_I2C_MST_STATUS 0x36 107 | #define MPU6050_RA_INT_PIN_CFG 0x37 108 | #define MPU6050_RA_INT_ENABLE 0x38 109 | #define MPU6050_RA_DMP_INT_STATUS 0x39 110 | #define MPU6050_RA_INT_STATUS 0x3A 111 | #define MPU6050_RA_ACCEL_XOUT_H 0x3B 112 | #define MPU6050_RA_ACCEL_XOUT_L 0x3C 113 | #define MPU6050_RA_ACCEL_YOUT_H 0x3D 114 | #define MPU6050_RA_ACCEL_YOUT_L 0x3E 115 | #define MPU6050_RA_ACCEL_ZOUT_H 0x3F 116 | #define MPU6050_RA_ACCEL_ZOUT_L 0x40 117 | #define MPU6050_RA_TEMP_OUT_H 0x41 118 | #define MPU6050_RA_TEMP_OUT_L 0x42 119 | #define MPU6050_RA_GYRO_XOUT_H 0x43 120 | #define MPU6050_RA_GYRO_XOUT_L 0x44 121 | #define MPU6050_RA_GYRO_YOUT_H 0x45 122 | #define MPU6050_RA_GYRO_YOUT_L 0x46 123 | #define MPU6050_RA_GYRO_ZOUT_H 0x47 124 | #define MPU6050_RA_GYRO_ZOUT_L 0x48 125 | #define MPU6050_RA_EXT_SENS_DATA_00 0x49 126 | #define MPU6050_RA_EXT_SENS_DATA_01 0x4A 127 | #define MPU6050_RA_EXT_SENS_DATA_02 0x4B 128 | #define MPU6050_RA_EXT_SENS_DATA_03 0x4C 129 | #define MPU6050_RA_EXT_SENS_DATA_04 0x4D 130 | #define MPU6050_RA_EXT_SENS_DATA_05 0x4E 131 | #define MPU6050_RA_EXT_SENS_DATA_06 0x4F 132 | #define MPU6050_RA_EXT_SENS_DATA_07 0x50 133 | #define MPU6050_RA_EXT_SENS_DATA_08 0x51 134 | #define MPU6050_RA_EXT_SENS_DATA_09 0x52 135 | #define MPU6050_RA_EXT_SENS_DATA_10 0x53 136 | #define MPU6050_RA_EXT_SENS_DATA_11 0x54 137 | #define MPU6050_RA_EXT_SENS_DATA_12 0x55 138 | #define MPU6050_RA_EXT_SENS_DATA_13 0x56 139 | #define MPU6050_RA_EXT_SENS_DATA_14 0x57 140 | #define MPU6050_RA_EXT_SENS_DATA_15 0x58 141 | #define MPU6050_RA_EXT_SENS_DATA_16 0x59 142 | #define MPU6050_RA_EXT_SENS_DATA_17 0x5A 143 | #define MPU6050_RA_EXT_SENS_DATA_18 0x5B 144 | #define MPU6050_RA_EXT_SENS_DATA_19 0x5C 145 | #define MPU6050_RA_EXT_SENS_DATA_20 0x5D 146 | #define MPU6050_RA_EXT_SENS_DATA_21 0x5E 147 | #define MPU6050_RA_EXT_SENS_DATA_22 0x5F 148 | #define MPU6050_RA_EXT_SENS_DATA_23 0x60 149 | #define MPU6050_RA_MOT_DETECT_STATUS 0x61 150 | #define MPU6050_RA_I2C_SLV0_DO 0x63 151 | #define MPU6050_RA_I2C_SLV1_DO 0x64 152 | #define MPU6050_RA_I2C_SLV2_DO 0x65 153 | #define MPU6050_RA_I2C_SLV3_DO 0x66 154 | #define MPU6050_RA_I2C_MST_DELAY_CTRL 0x67 155 | #define MPU6050_RA_SIGNAL_PATH_RESET 0x68 156 | #define MPU6050_RA_MOT_DETECT_CTRL 0x69 157 | #define MPU6050_RA_USER_CTRL 0x6A 158 | #define MPU6050_RA_PWR_MGMT_1 0x6B 159 | #define MPU6050_RA_PWR_MGMT_2 0x6C 160 | #define MPU6050_RA_BANK_SEL 0x6D 161 | #define MPU6050_RA_MEM_START_ADDR 0x6E 162 | #define MPU6050_RA_MEM_R_W 0x6F 163 | #define MPU6050_RA_DMP_CFG_1 0x70 164 | #define MPU6050_RA_DMP_CFG_2 0x71 165 | #define MPU6050_RA_FIFO_COUNTH 0x72 166 | #define MPU6050_RA_FIFO_COUNTL 0x73 167 | #define MPU6050_RA_FIFO_R_W 0x74 168 | #define MPU6050_RA_WHO_AM_I 0x75 169 | 170 | #define MPU6050_TC_PWR_MODE_BIT 7 171 | #define MPU6050_TC_OFFSET_BIT 6 172 | #define MPU6050_TC_OFFSET_LENGTH 6 173 | #define MPU6050_TC_OTP_BNK_VLD_BIT 0 174 | 175 | #define MPU6050_VDDIO_LEVEL_VLOGIC 0 176 | #define MPU6050_VDDIO_LEVEL_VDD 1 177 | 178 | #define MPU6050_CFG_EXT_SYNC_SET_BIT 5 179 | #define MPU6050_CFG_EXT_SYNC_SET_LENGTH 3 180 | #define MPU6050_CFG_DLPF_CFG_BIT 2 181 | #define MPU6050_CFG_DLPF_CFG_LENGTH 3 182 | 183 | #define MPU6050_EXT_SYNC_DISABLED 0x0 184 | #define MPU6050_EXT_SYNC_TEMP_OUT_L 0x1 185 | #define MPU6050_EXT_SYNC_GYRO_XOUT_L 0x2 186 | #define MPU6050_EXT_SYNC_GYRO_YOUT_L 0x3 187 | #define MPU6050_EXT_SYNC_GYRO_ZOUT_L 0x4 188 | #define MPU6050_EXT_SYNC_ACCEL_XOUT_L 0x5 189 | #define MPU6050_EXT_SYNC_ACCEL_YOUT_L 0x6 190 | #define MPU6050_EXT_SYNC_ACCEL_ZOUT_L 0x7 191 | 192 | #define MPU6050_DLPF_BW_256 0x00 193 | #define MPU6050_DLPF_BW_188 0x01 194 | #define MPU6050_DLPF_BW_98 0x02 195 | #define MPU6050_DLPF_BW_42 0x03 196 | #define MPU6050_DLPF_BW_20 0x04 197 | #define MPU6050_DLPF_BW_10 0x05 198 | #define MPU6050_DLPF_BW_5 0x06 199 | 200 | #define MPU6050_GCONFIG_FS_SEL_BIT 4 201 | #define MPU6050_GCONFIG_FS_SEL_LENGTH 2 202 | 203 | #define MPU6050_GYRO_FS_250 0x00 204 | #define MPU6050_GYRO_FS_500 0x01 205 | #define MPU6050_GYRO_FS_1000 0x02 206 | #define MPU6050_GYRO_FS_2000 0x03 207 | 208 | #define MPU6050_ACONFIG_XA_ST_BIT 7 209 | #define MPU6050_ACONFIG_YA_ST_BIT 6 210 | #define MPU6050_ACONFIG_ZA_ST_BIT 5 211 | #define MPU6050_ACONFIG_AFS_SEL_BIT 4 212 | #define MPU6050_ACONFIG_AFS_SEL_LENGTH 2 213 | #define MPU6050_ACONFIG_ACCEL_HPF_BIT 2 214 | #define MPU6050_ACONFIG_ACCEL_HPF_LENGTH 3 215 | 216 | #define MPU6050_ACCEL_FS_2 0x00 217 | #define MPU6050_ACCEL_FS_4 0x01 218 | #define MPU6050_ACCEL_FS_8 0x02 219 | #define MPU6050_ACCEL_FS_16 0x03 220 | 221 | #define MPU6050_DHPF_RESET 0x00 222 | #define MPU6050_DHPF_5 0x01 223 | #define MPU6050_DHPF_2P5 0x02 224 | #define MPU6050_DHPF_1P25 0x03 225 | #define MPU6050_DHPF_0P63 0x04 226 | #define MPU6050_DHPF_HOLD 0x07 227 | 228 | #define MPU6050_TEMP_FIFO_EN_BIT 7 229 | #define MPU6050_XG_FIFO_EN_BIT 6 230 | #define MPU6050_YG_FIFO_EN_BIT 5 231 | #define MPU6050_ZG_FIFO_EN_BIT 4 232 | #define MPU6050_ACCEL_FIFO_EN_BIT 3 233 | #define MPU6050_SLV2_FIFO_EN_BIT 2 234 | #define MPU6050_SLV1_FIFO_EN_BIT 1 235 | #define MPU6050_SLV0_FIFO_EN_BIT 0 236 | 237 | #define MPU6050_MULT_MST_EN_BIT 7 238 | #define MPU6050_WAIT_FOR_ES_BIT 6 239 | #define MPU6050_SLV_3_FIFO_EN_BIT 5 240 | #define MPU6050_I2C_MST_P_NSR_BIT 4 241 | #define MPU6050_I2C_MST_CLK_BIT 3 242 | #define MPU6050_I2C_MST_CLK_LENGTH 4 243 | 244 | #define MPU6050_CLOCK_DIV_348 0x0 245 | #define MPU6050_CLOCK_DIV_333 0x1 246 | #define MPU6050_CLOCK_DIV_320 0x2 247 | #define MPU6050_CLOCK_DIV_308 0x3 248 | #define MPU6050_CLOCK_DIV_296 0x4 249 | #define MPU6050_CLOCK_DIV_286 0x5 250 | #define MPU6050_CLOCK_DIV_276 0x6 251 | #define MPU6050_CLOCK_DIV_267 0x7 252 | #define MPU6050_CLOCK_DIV_258 0x8 253 | #define MPU6050_CLOCK_DIV_500 0x9 254 | #define MPU6050_CLOCK_DIV_471 0xA 255 | #define MPU6050_CLOCK_DIV_444 0xB 256 | #define MPU6050_CLOCK_DIV_421 0xC 257 | #define MPU6050_CLOCK_DIV_400 0xD 258 | #define MPU6050_CLOCK_DIV_381 0xE 259 | #define MPU6050_CLOCK_DIV_364 0xF 260 | 261 | #define MPU6050_I2C_SLV_RW_BIT 7 262 | #define MPU6050_I2C_SLV_ADDR_BIT 6 263 | #define MPU6050_I2C_SLV_ADDR_LENGTH 7 264 | #define MPU6050_I2C_SLV_EN_BIT 7 265 | #define MPU6050_I2C_SLV_BYTE_SW_BIT 6 266 | #define MPU6050_I2C_SLV_REG_DIS_BIT 5 267 | #define MPU6050_I2C_SLV_GRP_BIT 4 268 | #define MPU6050_I2C_SLV_LEN_BIT 3 269 | #define MPU6050_I2C_SLV_LEN_LENGTH 4 270 | 271 | #define MPU6050_I2C_SLV4_RW_BIT 7 272 | #define MPU6050_I2C_SLV4_ADDR_BIT 6 273 | #define MPU6050_I2C_SLV4_ADDR_LENGTH 7 274 | #define MPU6050_I2C_SLV4_EN_BIT 7 275 | #define MPU6050_I2C_SLV4_INT_EN_BIT 6 276 | #define MPU6050_I2C_SLV4_REG_DIS_BIT 5 277 | #define MPU6050_I2C_SLV4_MST_DLY_BIT 4 278 | #define MPU6050_I2C_SLV4_MST_DLY_LENGTH 5 279 | 280 | #define MPU6050_MST_PASS_THROUGH_BIT 7 281 | #define MPU6050_MST_I2C_SLV4_DONE_BIT 6 282 | #define MPU6050_MST_I2C_LOST_ARB_BIT 5 283 | #define MPU6050_MST_I2C_SLV4_NACK_BIT 4 284 | #define MPU6050_MST_I2C_SLV3_NACK_BIT 3 285 | #define MPU6050_MST_I2C_SLV2_NACK_BIT 2 286 | #define MPU6050_MST_I2C_SLV1_NACK_BIT 1 287 | #define MPU6050_MST_I2C_SLV0_NACK_BIT 0 288 | 289 | #define MPU6050_INTCFG_INT_LEVEL_BIT 7 290 | #define MPU6050_INTCFG_INT_OPEN_BIT 6 291 | #define MPU6050_INTCFG_LATCH_INT_EN_BIT 5 292 | #define MPU6050_INTCFG_INT_RD_CLEAR_BIT 4 293 | #define MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT 3 294 | #define MPU6050_INTCFG_FSYNC_INT_EN_BIT 2 295 | #define MPU6050_INTCFG_I2C_BYPASS_EN_BIT 1 296 | #define MPU6050_INTCFG_CLKOUT_EN_BIT 0 297 | 298 | #define MPU6050_INTMODE_ACTIVEHIGH 0x00 299 | #define MPU6050_INTMODE_ACTIVELOW 0x01 300 | 301 | #define MPU6050_INTDRV_PUSHPULL 0x00 302 | #define MPU6050_INTDRV_OPENDRAIN 0x01 303 | 304 | #define MPU6050_INTLATCH_50USPULSE 0x00 305 | #define MPU6050_INTLATCH_WAITCLEAR 0x01 306 | 307 | #define MPU6050_INTCLEAR_STATUSREAD 0x00 308 | #define MPU6050_INTCLEAR_ANYREAD 0x01 309 | 310 | #define MPU6050_INTERRUPT_FF_BIT 7 311 | #define MPU6050_INTERRUPT_MOT_BIT 6 312 | #define MPU6050_INTERRUPT_ZMOT_BIT 5 313 | #define MPU6050_INTERRUPT_FIFO_OFLOW_BIT 4 314 | #define MPU6050_INTERRUPT_I2C_MST_INT_BIT 3 315 | #define MPU6050_INTERRUPT_PLL_RDY_INT_BIT 2 316 | #define MPU6050_INTERRUPT_DMP_INT_BIT 1 317 | #define MPU6050_INTERRUPT_DATA_RDY_BIT 0 318 | 319 | // TODO: figure out what these actually do 320 | // UMPL source code is not very obivous 321 | #define MPU6050_DMPINT_5_BIT 5 322 | #define MPU6050_DMPINT_4_BIT 4 323 | #define MPU6050_DMPINT_3_BIT 3 324 | #define MPU6050_DMPINT_2_BIT 2 325 | #define MPU6050_DMPINT_1_BIT 1 326 | #define MPU6050_DMPINT_0_BIT 0 327 | 328 | #define MPU6050_MOTION_MOT_XNEG_BIT 7 329 | #define MPU6050_MOTION_MOT_XPOS_BIT 6 330 | #define MPU6050_MOTION_MOT_YNEG_BIT 5 331 | #define MPU6050_MOTION_MOT_YPOS_BIT 4 332 | #define MPU6050_MOTION_MOT_ZNEG_BIT 3 333 | #define MPU6050_MOTION_MOT_ZPOS_BIT 2 334 | #define MPU6050_MOTION_MOT_ZRMOT_BIT 0 335 | 336 | #define MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT 7 337 | #define MPU6050_DELAYCTRL_I2C_SLV4_DLY_EN_BIT 4 338 | #define MPU6050_DELAYCTRL_I2C_SLV3_DLY_EN_BIT 3 339 | #define MPU6050_DELAYCTRL_I2C_SLV2_DLY_EN_BIT 2 340 | #define MPU6050_DELAYCTRL_I2C_SLV1_DLY_EN_BIT 1 341 | #define MPU6050_DELAYCTRL_I2C_SLV0_DLY_EN_BIT 0 342 | 343 | #define MPU6050_PATHRESET_GYRO_RESET_BIT 2 344 | #define MPU6050_PATHRESET_ACCEL_RESET_BIT 1 345 | #define MPU6050_PATHRESET_TEMP_RESET_BIT 0 346 | 347 | #define MPU6050_DETECT_ACCEL_ON_DELAY_BIT 5 348 | #define MPU6050_DETECT_ACCEL_ON_DELAY_LENGTH 2 349 | #define MPU6050_DETECT_FF_COUNT_BIT 3 350 | #define MPU6050_DETECT_FF_COUNT_LENGTH 2 351 | #define MPU6050_DETECT_MOT_COUNT_BIT 1 352 | #define MPU6050_DETECT_MOT_COUNT_LENGTH 2 353 | 354 | #define MPU6050_DETECT_DECREMENT_RESET 0x0 355 | #define MPU6050_DETECT_DECREMENT_1 0x1 356 | #define MPU6050_DETECT_DECREMENT_2 0x2 357 | #define MPU6050_DETECT_DECREMENT_4 0x3 358 | 359 | #define MPU6050_USERCTRL_DMP_EN_BIT 7 360 | #define MPU6050_USERCTRL_FIFO_EN_BIT 6 361 | #define MPU6050_USERCTRL_I2C_MST_EN_BIT 5 362 | #define MPU6050_USERCTRL_I2C_IF_DIS_BIT 4 363 | #define MPU6050_USERCTRL_DMP_RESET_BIT 3 364 | #define MPU6050_USERCTRL_FIFO_RESET_BIT 2 365 | #define MPU6050_USERCTRL_I2C_MST_RESET_BIT 1 366 | #define MPU6050_USERCTRL_SIG_COND_RESET_BIT 0 367 | 368 | #define MPU6050_PWR1_DEVICE_RESET_BIT 7 369 | #define MPU6050_PWR1_SLEEP_BIT 6 370 | #define MPU6050_PWR1_CYCLE_BIT 5 371 | #define MPU6050_PWR1_TEMP_DIS_BIT 3 372 | #define MPU6050_PWR1_CLKSEL_BIT 2 373 | #define MPU6050_PWR1_CLKSEL_LENGTH 3 374 | 375 | #define MPU6050_CLOCK_INTERNAL 0x00 376 | #define MPU6050_CLOCK_PLL_XGYRO 0x01 377 | #define MPU6050_CLOCK_PLL_YGYRO 0x02 378 | #define MPU6050_CLOCK_PLL_ZGYRO 0x03 379 | #define MPU6050_CLOCK_PLL_EXT32K 0x04 380 | #define MPU6050_CLOCK_PLL_EXT19M 0x05 381 | #define MPU6050_CLOCK_KEEP_RESET 0x07 382 | 383 | #define MPU6050_PWR2_LP_WAKE_CTRL_BIT 7 384 | #define MPU6050_PWR2_LP_WAKE_CTRL_LENGTH 2 385 | #define MPU6050_PWR2_STBY_XA_BIT 5 386 | #define MPU6050_PWR2_STBY_YA_BIT 4 387 | #define MPU6050_PWR2_STBY_ZA_BIT 3 388 | #define MPU6050_PWR2_STBY_XG_BIT 2 389 | #define MPU6050_PWR2_STBY_YG_BIT 1 390 | #define MPU6050_PWR2_STBY_ZG_BIT 0 391 | 392 | #define MPU6050_WAKE_FREQ_1P25 0x0 393 | #define MPU6050_WAKE_FREQ_2P5 0x1 394 | #define MPU6050_WAKE_FREQ_5 0x2 395 | #define MPU6050_WAKE_FREQ_10 0x3 396 | 397 | #define MPU6050_BANKSEL_PRFTCH_EN_BIT 6 398 | #define MPU6050_BANKSEL_CFG_USER_BANK_BIT 5 399 | #define MPU6050_BANKSEL_MEM_SEL_BIT 4 400 | #define MPU6050_BANKSEL_MEM_SEL_LENGTH 5 401 | 402 | #define MPU6050_WHO_AM_I_BIT 6 403 | #define MPU6050_WHO_AM_I_LENGTH 6 404 | 405 | #define MPU6050_DMP_MEMORY_BANKS 8 406 | #define MPU6050_DMP_MEMORY_BANK_SIZE 256 407 | #define MPU6050_DMP_MEMORY_CHUNK_SIZE 16 408 | 409 | // note: DMP code memory blocks defined at end of header file 410 | 411 | class MPU6050 { 412 | public: 413 | MPU6050(); 414 | MPU6050(uint8_t address); 415 | 416 | void initialize(); 417 | bool testConnection(); 418 | 419 | // AUX_VDDIO register 420 | uint8_t getAuxVDDIOLevel(); 421 | void setAuxVDDIOLevel(uint8_t level); 422 | 423 | // SMPLRT_DIV register 424 | uint8_t getRate(); 425 | void setRate(uint8_t rate); 426 | 427 | // CONFIG register 428 | uint8_t getExternalFrameSync(); 429 | void setExternalFrameSync(uint8_t sync); 430 | uint8_t getDLPFMode(); 431 | void setDLPFMode(uint8_t bandwidth); 432 | 433 | // GYRO_CONFIG register 434 | uint8_t getFullScaleGyroRange(); 435 | void setFullScaleGyroRange(uint8_t range); 436 | 437 | // ACCEL_CONFIG register 438 | bool getAccelXSelfTest(); 439 | void setAccelXSelfTest(bool enabled); 440 | bool getAccelYSelfTest(); 441 | void setAccelYSelfTest(bool enabled); 442 | bool getAccelZSelfTest(); 443 | void setAccelZSelfTest(bool enabled); 444 | uint8_t getFullScaleAccelRange(); 445 | void setFullScaleAccelRange(uint8_t range); 446 | uint8_t getDHPFMode(); 447 | void setDHPFMode(uint8_t mode); 448 | 449 | // FF_THR register 450 | uint8_t getFreefallDetectionThreshold(); 451 | void setFreefallDetectionThreshold(uint8_t threshold); 452 | 453 | // FF_DUR register 454 | uint8_t getFreefallDetectionDuration(); 455 | void setFreefallDetectionDuration(uint8_t duration); 456 | 457 | // MOT_THR register 458 | uint8_t getMotionDetectionThreshold(); 459 | void setMotionDetectionThreshold(uint8_t threshold); 460 | 461 | // MOT_DUR register 462 | uint8_t getMotionDetectionDuration(); 463 | void setMotionDetectionDuration(uint8_t duration); 464 | 465 | // ZRMOT_THR register 466 | uint8_t getZeroMotionDetectionThreshold(); 467 | void setZeroMotionDetectionThreshold(uint8_t threshold); 468 | 469 | // ZRMOT_DUR register 470 | uint8_t getZeroMotionDetectionDuration(); 471 | void setZeroMotionDetectionDuration(uint8_t duration); 472 | 473 | // FIFO_EN register 474 | bool getTempFIFOEnabled(); 475 | void setTempFIFOEnabled(bool enabled); 476 | bool getXGyroFIFOEnabled(); 477 | void setXGyroFIFOEnabled(bool enabled); 478 | bool getYGyroFIFOEnabled(); 479 | void setYGyroFIFOEnabled(bool enabled); 480 | bool getZGyroFIFOEnabled(); 481 | void setZGyroFIFOEnabled(bool enabled); 482 | bool getAccelFIFOEnabled(); 483 | void setAccelFIFOEnabled(bool enabled); 484 | bool getSlave2FIFOEnabled(); 485 | void setSlave2FIFOEnabled(bool enabled); 486 | bool getSlave1FIFOEnabled(); 487 | void setSlave1FIFOEnabled(bool enabled); 488 | bool getSlave0FIFOEnabled(); 489 | void setSlave0FIFOEnabled(bool enabled); 490 | 491 | // I2C_MST_CTRL register 492 | bool getMultiMasterEnabled(); 493 | void setMultiMasterEnabled(bool enabled); 494 | bool getWaitForExternalSensorEnabled(); 495 | void setWaitForExternalSensorEnabled(bool enabled); 496 | bool getSlave3FIFOEnabled(); 497 | void setSlave3FIFOEnabled(bool enabled); 498 | bool getSlaveReadWriteTransitionEnabled(); 499 | void setSlaveReadWriteTransitionEnabled(bool enabled); 500 | uint8_t getMasterClockSpeed(); 501 | void setMasterClockSpeed(uint8_t speed); 502 | 503 | // I2C_SLV* registers (Slave 0-3) 504 | uint8_t getSlaveAddress(uint8_t num); 505 | void setSlaveAddress(uint8_t num, uint8_t address); 506 | uint8_t getSlaveRegister(uint8_t num); 507 | void setSlaveRegister(uint8_t num, uint8_t reg); 508 | bool getSlaveEnabled(uint8_t num); 509 | void setSlaveEnabled(uint8_t num, bool enabled); 510 | bool getSlaveWordByteSwap(uint8_t num); 511 | void setSlaveWordByteSwap(uint8_t num, bool enabled); 512 | bool getSlaveWriteMode(uint8_t num); 513 | void setSlaveWriteMode(uint8_t num, bool mode); 514 | bool getSlaveWordGroupOffset(uint8_t num); 515 | void setSlaveWordGroupOffset(uint8_t num, bool enabled); 516 | uint8_t getSlaveDataLength(uint8_t num); 517 | void setSlaveDataLength(uint8_t num, uint8_t length); 518 | 519 | // I2C_SLV* registers (Slave 4) 520 | uint8_t getSlave4Address(); 521 | void setSlave4Address(uint8_t address); 522 | uint8_t getSlave4Register(); 523 | void setSlave4Register(uint8_t reg); 524 | void setSlave4OutputByte(uint8_t data); 525 | bool getSlave4Enabled(); 526 | void setSlave4Enabled(bool enabled); 527 | bool getSlave4InterruptEnabled(); 528 | void setSlave4InterruptEnabled(bool enabled); 529 | bool getSlave4WriteMode(); 530 | void setSlave4WriteMode(bool mode); 531 | uint8_t getSlave4MasterDelay(); 532 | void setSlave4MasterDelay(uint8_t delay); 533 | uint8_t getSlate4InputByte(); 534 | 535 | // I2C_MST_STATUS register 536 | bool getPassthroughStatus(); 537 | bool getSlave4IsDone(); 538 | bool getLostArbitration(); 539 | bool getSlave4Nack(); 540 | bool getSlave3Nack(); 541 | bool getSlave2Nack(); 542 | bool getSlave1Nack(); 543 | bool getSlave0Nack(); 544 | 545 | // INT_PIN_CFG register 546 | bool getInterruptMode(); 547 | void setInterruptMode(bool mode); 548 | bool getInterruptDrive(); 549 | void setInterruptDrive(bool drive); 550 | bool getInterruptLatch(); 551 | void setInterruptLatch(bool latch); 552 | bool getInterruptLatchClear(); 553 | void setInterruptLatchClear(bool clear); 554 | bool getFSyncInterruptLevel(); 555 | void setFSyncInterruptLevel(bool level); 556 | bool getFSyncInterruptEnabled(); 557 | void setFSyncInterruptEnabled(bool enabled); 558 | bool getI2CBypassEnabled(); 559 | void setI2CBypassEnabled(bool enabled); 560 | bool getClockOutputEnabled(); 561 | void setClockOutputEnabled(bool enabled); 562 | 563 | // INT_ENABLE register 564 | uint8_t getIntEnabled(); 565 | void setIntEnabled(uint8_t enabled); 566 | bool getIntFreefallEnabled(); 567 | void setIntFreefallEnabled(bool enabled); 568 | bool getIntMotionEnabled(); 569 | void setIntMotionEnabled(bool enabled); 570 | bool getIntZeroMotionEnabled(); 571 | void setIntZeroMotionEnabled(bool enabled); 572 | bool getIntFIFOBufferOverflowEnabled(); 573 | void setIntFIFOBufferOverflowEnabled(bool enabled); 574 | bool getIntI2CMasterEnabled(); 575 | void setIntI2CMasterEnabled(bool enabled); 576 | bool getIntDataReadyEnabled(); 577 | void setIntDataReadyEnabled(bool enabled); 578 | 579 | // INT_STATUS register 580 | uint8_t getIntStatus(); 581 | bool getIntFreefallStatus(); 582 | bool getIntMotionStatus(); 583 | bool getIntZeroMotionStatus(); 584 | bool getIntFIFOBufferOverflowStatus(); 585 | bool getIntI2CMasterStatus(); 586 | bool getIntDataReadyStatus(); 587 | 588 | // ACCEL_*OUT_* registers 589 | 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); 590 | void getMotion6(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz); 591 | void getAcceleration(int16_t* x, int16_t* y, int16_t* z); 592 | int16_t getAccelerationX(); 593 | int16_t getAccelerationY(); 594 | int16_t getAccelerationZ(); 595 | 596 | // TEMP_OUT_* registers 597 | int16_t getTemperature(); 598 | 599 | // GYRO_*OUT_* registers 600 | void getRotation(int16_t* x, int16_t* y, int16_t* z); 601 | int16_t getRotationX(); 602 | int16_t getRotationY(); 603 | int16_t getRotationZ(); 604 | 605 | // EXT_SENS_DATA_* registers 606 | uint8_t getExternalSensorByte(int position); 607 | uint16_t getExternalSensorWord(int position); 608 | uint32_t getExternalSensorDWord(int position); 609 | 610 | // MOT_DETECT_STATUS register 611 | bool getXNegMotionDetected(); 612 | bool getXPosMotionDetected(); 613 | bool getYNegMotionDetected(); 614 | bool getYPosMotionDetected(); 615 | bool getZNegMotionDetected(); 616 | bool getZPosMotionDetected(); 617 | bool getZeroMotionDetected(); 618 | 619 | // I2C_SLV*_DO register 620 | void setSlaveOutputByte(uint8_t num, uint8_t data); 621 | 622 | // I2C_MST_DELAY_CTRL register 623 | bool getExternalShadowDelayEnabled(); 624 | void setExternalShadowDelayEnabled(bool enabled); 625 | bool getSlaveDelayEnabled(uint8_t num); 626 | void setSlaveDelayEnabled(uint8_t num, bool enabled); 627 | 628 | // SIGNAL_PATH_RESET register 629 | void resetGyroscopePath(); 630 | void resetAccelerometerPath(); 631 | void resetTemperaturePath(); 632 | 633 | // MOT_DETECT_CTRL register 634 | uint8_t getAccelerometerPowerOnDelay(); 635 | void setAccelerometerPowerOnDelay(uint8_t delay); 636 | uint8_t getFreefallDetectionCounterDecrement(); 637 | void setFreefallDetectionCounterDecrement(uint8_t decrement); 638 | uint8_t getMotionDetectionCounterDecrement(); 639 | void setMotionDetectionCounterDecrement(uint8_t decrement); 640 | 641 | // USER_CTRL register 642 | bool getFIFOEnabled(); 643 | void setFIFOEnabled(bool enabled); 644 | bool getI2CMasterModeEnabled(); 645 | void setI2CMasterModeEnabled(bool enabled); 646 | void switchSPIEnabled(bool enabled); 647 | void resetFIFO(); 648 | void resetI2CMaster(); 649 | void resetSensors(); 650 | 651 | // PWR_MGMT_1 register 652 | void reset(); 653 | bool getSleepEnabled(); 654 | void setSleepEnabled(bool enabled); 655 | bool getWakeCycleEnabled(); 656 | void setWakeCycleEnabled(bool enabled); 657 | bool getTempSensorEnabled(); 658 | void setTempSensorEnabled(bool enabled); 659 | uint8_t getClockSource(); 660 | void setClockSource(uint8_t source); 661 | 662 | // PWR_MGMT_2 register 663 | uint8_t getWakeFrequency(); 664 | void setWakeFrequency(uint8_t frequency); 665 | bool getStandbyXAccelEnabled(); 666 | void setStandbyXAccelEnabled(bool enabled); 667 | bool getStandbyYAccelEnabled(); 668 | void setStandbyYAccelEnabled(bool enabled); 669 | bool getStandbyZAccelEnabled(); 670 | void setStandbyZAccelEnabled(bool enabled); 671 | bool getStandbyXGyroEnabled(); 672 | void setStandbyXGyroEnabled(bool enabled); 673 | bool getStandbyYGyroEnabled(); 674 | void setStandbyYGyroEnabled(bool enabled); 675 | bool getStandbyZGyroEnabled(); 676 | void setStandbyZGyroEnabled(bool enabled); 677 | 678 | // FIFO_COUNT_* registers 679 | uint16_t getFIFOCount(); 680 | 681 | // FIFO_R_W register 682 | uint8_t getFIFOByte(); 683 | void setFIFOByte(uint8_t data); 684 | void getFIFOBytes(uint8_t *data, uint8_t length); 685 | 686 | // WHO_AM_I register 687 | uint8_t getDeviceID(); 688 | void setDeviceID(uint8_t id); 689 | 690 | // ======== UNDOCUMENTED/DMP REGISTERS/METHODS ======== 691 | 692 | // XG_OFFS_TC register 693 | uint8_t getOTPBankValid(); 694 | void setOTPBankValid(bool enabled); 695 | int8_t getXGyroOffsetTC(); 696 | void setXGyroOffsetTC(int8_t offset); 697 | 698 | // YG_OFFS_TC register 699 | int8_t getYGyroOffsetTC(); 700 | void setYGyroOffsetTC(int8_t offset); 701 | 702 | // ZG_OFFS_TC register 703 | int8_t getZGyroOffsetTC(); 704 | void setZGyroOffsetTC(int8_t offset); 705 | 706 | // X_FINE_GAIN register 707 | int8_t getXFineGain(); 708 | void setXFineGain(int8_t gain); 709 | 710 | // Y_FINE_GAIN register 711 | int8_t getYFineGain(); 712 | void setYFineGain(int8_t gain); 713 | 714 | // Z_FINE_GAIN register 715 | int8_t getZFineGain(); 716 | void setZFineGain(int8_t gain); 717 | 718 | // XA_OFFS_* registers 719 | int16_t getXAccelOffset(); 720 | void setXAccelOffset(int16_t offset); 721 | 722 | // YA_OFFS_* register 723 | int16_t getYAccelOffset(); 724 | void setYAccelOffset(int16_t offset); 725 | 726 | // ZA_OFFS_* register 727 | int16_t getZAccelOffset(); 728 | void setZAccelOffset(int16_t offset); 729 | 730 | // XG_OFFS_USR* registers 731 | int16_t getXGyroOffset(); 732 | void setXGyroOffset(int16_t offset); 733 | 734 | // YG_OFFS_USR* register 735 | int16_t getYGyroOffset(); 736 | void setYGyroOffset(int16_t offset); 737 | 738 | // ZG_OFFS_USR* register 739 | int16_t getZGyroOffset(); 740 | void setZGyroOffset(int16_t offset); 741 | 742 | // INT_ENABLE register (DMP functions) 743 | bool getIntPLLReadyEnabled(); 744 | void setIntPLLReadyEnabled(bool enabled); 745 | bool getIntDMPEnabled(); 746 | void setIntDMPEnabled(bool enabled); 747 | 748 | // DMP_INT_STATUS 749 | bool getDMPInt5Status(); 750 | bool getDMPInt4Status(); 751 | bool getDMPInt3Status(); 752 | bool getDMPInt2Status(); 753 | bool getDMPInt1Status(); 754 | bool getDMPInt0Status(); 755 | 756 | // INT_STATUS register (DMP functions) 757 | bool getIntPLLReadyStatus(); 758 | bool getIntDMPStatus(); 759 | 760 | // USER_CTRL register (DMP functions) 761 | bool getDMPEnabled(); 762 | void setDMPEnabled(bool enabled); 763 | void resetDMP(); 764 | 765 | // BANK_SEL register 766 | void setMemoryBank(uint8_t bank, bool prefetchEnabled=false, bool userBank=false); 767 | 768 | // MEM_START_ADDR register 769 | void setMemoryStartAddress(uint8_t address); 770 | 771 | // MEM_R_W register 772 | uint8_t readMemoryByte(); 773 | void writeMemoryByte(uint8_t data); 774 | void readMemoryBlock(uint8_t *data, uint16_t dataSize, uint8_t bank=0, uint8_t address=0); 775 | bool writeMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank=0, uint8_t address=0, bool verify=true, bool useProgMem=false); 776 | bool writeProgMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank=0, uint8_t address=0, bool verify=true); 777 | 778 | bool writeDMPConfigurationSet(const uint8_t *data, uint16_t dataSize, bool useProgMem=false); 779 | bool writeProgDMPConfigurationSet(const uint8_t *data, uint16_t dataSize); 780 | 781 | // DMP_CFG_1 register 782 | uint8_t getDMPConfig1(); 783 | void setDMPConfig1(uint8_t config); 784 | 785 | // DMP_CFG_2 register 786 | uint8_t getDMPConfig2(); 787 | void setDMPConfig2(uint8_t config); 788 | 789 | // special methods for MotionApps 2.0 implementation 790 | #ifdef MPU6050_INCLUDE_DMP_MOTIONAPPS20 791 | uint8_t *dmpPacketBuffer; 792 | uint16_t dmpPacketSize; 793 | 794 | uint8_t dmpInitialize(); 795 | bool dmpPacketAvailable(); 796 | 797 | uint8_t dmpSetFIFORate(uint8_t fifoRate); 798 | uint8_t dmpGetFIFORate(); 799 | uint8_t dmpGetSampleStepSizeMS(); 800 | uint8_t dmpGetSampleFrequency(); 801 | int32_t dmpDecodeTemperature(int8_t tempReg); 802 | 803 | // Register callbacks after a packet of FIFO data is processed 804 | //uint8_t dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority); 805 | //uint8_t dmpUnregisterFIFORateProcess(inv_obj_func func); 806 | uint8_t dmpRunFIFORateProcesses(); 807 | 808 | // Setup FIFO for various output 809 | uint8_t dmpSendQuaternion(uint_fast16_t accuracy); 810 | uint8_t dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy); 811 | uint8_t dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy); 812 | uint8_t dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy); 813 | uint8_t dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy); 814 | uint8_t dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy); 815 | uint8_t dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy); 816 | uint8_t dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy); 817 | uint8_t dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy); 818 | uint8_t dmpSendPacketNumber(uint_fast16_t accuracy); 819 | uint8_t dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy); 820 | uint8_t dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy); 821 | 822 | // Get Fixed Point data from FIFO 823 | uint8_t dmpGetAccel(int32_t *data, const uint8_t* packet=0); 824 | uint8_t dmpGetAccel(int16_t *data, const uint8_t* packet=0); 825 | uint8_t dmpGetAccel(VectorInt16 *v, const uint8_t* packet=0); 826 | uint8_t dmpGetQuaternion(int32_t *data, const uint8_t* packet=0); 827 | uint8_t dmpGetQuaternion(int16_t *data, const uint8_t* packet=0); 828 | uint8_t dmpGetQuaternion(Quaternion *q, const uint8_t* packet=0); 829 | uint8_t dmpGet6AxisQuaternion(int32_t *data, const uint8_t* packet=0); 830 | uint8_t dmpGet6AxisQuaternion(int16_t *data, const uint8_t* packet=0); 831 | uint8_t dmpGet6AxisQuaternion(Quaternion *q, const uint8_t* packet=0); 832 | uint8_t dmpGetRelativeQuaternion(int32_t *data, const uint8_t* packet=0); 833 | uint8_t dmpGetRelativeQuaternion(int16_t *data, const uint8_t* packet=0); 834 | uint8_t dmpGetRelativeQuaternion(Quaternion *data, const uint8_t* packet=0); 835 | uint8_t dmpGetGyro(int32_t *data, const uint8_t* packet=0); 836 | uint8_t dmpGetGyro(int16_t *data, const uint8_t* packet=0); 837 | uint8_t dmpGetGyro(VectorInt16 *v, const uint8_t* packet=0); 838 | uint8_t dmpSetLinearAccelFilterCoefficient(float coef); 839 | uint8_t dmpGetLinearAccel(int32_t *data, const uint8_t* packet=0); 840 | uint8_t dmpGetLinearAccel(int16_t *data, const uint8_t* packet=0); 841 | uint8_t dmpGetLinearAccel(VectorInt16 *v, const uint8_t* packet=0); 842 | uint8_t dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity); 843 | uint8_t dmpGetLinearAccelInWorld(int32_t *data, const uint8_t* packet=0); 844 | uint8_t dmpGetLinearAccelInWorld(int16_t *data, const uint8_t* packet=0); 845 | uint8_t dmpGetLinearAccelInWorld(VectorInt16 *v, const uint8_t* packet=0); 846 | uint8_t dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q); 847 | uint8_t dmpGetGyroAndAccelSensor(int32_t *data, const uint8_t* packet=0); 848 | uint8_t dmpGetGyroAndAccelSensor(int16_t *data, const uint8_t* packet=0); 849 | uint8_t dmpGetGyroAndAccelSensor(VectorInt16 *g, VectorInt16 *a, const uint8_t* packet=0); 850 | uint8_t dmpGetGyroSensor(int32_t *data, const uint8_t* packet=0); 851 | uint8_t dmpGetGyroSensor(int16_t *data, const uint8_t* packet=0); 852 | uint8_t dmpGetGyroSensor(VectorInt16 *v, const uint8_t* packet=0); 853 | uint8_t dmpGetControlData(int32_t *data, const uint8_t* packet=0); 854 | uint8_t dmpGetTemperature(int32_t *data, const uint8_t* packet=0); 855 | uint8_t dmpGetGravity(int32_t *data, const uint8_t* packet=0); 856 | uint8_t dmpGetGravity(int16_t *data, const uint8_t* packet=0); 857 | uint8_t dmpGetGravity(VectorInt16 *v, const uint8_t* packet=0); 858 | uint8_t dmpGetGravity(VectorFloat *v, Quaternion *q); 859 | uint8_t dmpGetUnquantizedAccel(int32_t *data, const uint8_t* packet=0); 860 | uint8_t dmpGetUnquantizedAccel(int16_t *data, const uint8_t* packet=0); 861 | uint8_t dmpGetUnquantizedAccel(VectorInt16 *v, const uint8_t* packet=0); 862 | uint8_t dmpGetQuantizedAccel(int32_t *data, const uint8_t* packet=0); 863 | uint8_t dmpGetQuantizedAccel(int16_t *data, const uint8_t* packet=0); 864 | uint8_t dmpGetQuantizedAccel(VectorInt16 *v, const uint8_t* packet=0); 865 | uint8_t dmpGetExternalSensorData(int32_t *data, uint16_t size, const uint8_t* packet=0); 866 | uint8_t dmpGetEIS(int32_t *data, const uint8_t* packet=0); 867 | 868 | uint8_t dmpGetEuler(float *data, Quaternion *q); 869 | uint8_t dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity); 870 | 871 | // Get Floating Point data from FIFO 872 | uint8_t dmpGetAccelFloat(float *data, const uint8_t* packet=0); 873 | uint8_t dmpGetQuaternionFloat(float *data, const uint8_t* packet=0); 874 | 875 | uint8_t dmpProcessFIFOPacket(const unsigned char *dmpData); 876 | uint8_t dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed=NULL); 877 | 878 | uint8_t dmpSetFIFOProcessedCallback(void (*func) (void)); 879 | 880 | uint8_t dmpInitFIFOParam(); 881 | uint8_t dmpCloseFIFO(); 882 | uint8_t dmpSetGyroDataSource(uint8_t source); 883 | uint8_t dmpDecodeQuantizedAccel(); 884 | uint32_t dmpGetGyroSumOfSquare(); 885 | uint32_t dmpGetAccelSumOfSquare(); 886 | void dmpOverrideQuaternion(long *q); 887 | uint16_t dmpGetFIFOPacketSize(); 888 | #endif 889 | 890 | // special methods for MotionApps 4.1 implementation 891 | #ifdef MPU6050_INCLUDE_DMP_MOTIONAPPS41 892 | uint8_t *dmpPacketBuffer; 893 | uint16_t dmpPacketSize; 894 | 895 | uint8_t dmpInitialize(); 896 | bool dmpPacketAvailable(); 897 | 898 | uint8_t dmpSetFIFORate(uint8_t fifoRate); 899 | uint8_t dmpGetFIFORate(); 900 | uint8_t dmpGetSampleStepSizeMS(); 901 | uint8_t dmpGetSampleFrequency(); 902 | int32_t dmpDecodeTemperature(int8_t tempReg); 903 | 904 | // Register callbacks after a packet of FIFO data is processed 905 | //uint8_t dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority); 906 | //uint8_t dmpUnregisterFIFORateProcess(inv_obj_func func); 907 | uint8_t dmpRunFIFORateProcesses(); 908 | 909 | // Setup FIFO for various output 910 | uint8_t dmpSendQuaternion(uint_fast16_t accuracy); 911 | uint8_t dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy); 912 | uint8_t dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy); 913 | uint8_t dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy); 914 | uint8_t dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy); 915 | uint8_t dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy); 916 | uint8_t dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy); 917 | uint8_t dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy); 918 | uint8_t dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy); 919 | uint8_t dmpSendPacketNumber(uint_fast16_t accuracy); 920 | uint8_t dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy); 921 | uint8_t dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy); 922 | 923 | // Get Fixed Point data from FIFO 924 | uint8_t dmpGetAccel(int32_t *data, const uint8_t* packet=0); 925 | uint8_t dmpGetAccel(int16_t *data, const uint8_t* packet=0); 926 | uint8_t dmpGetAccel(VectorInt16 *v, const uint8_t* packet=0); 927 | uint8_t dmpGetQuaternion(int32_t *data, const uint8_t* packet=0); 928 | uint8_t dmpGetQuaternion(int16_t *data, const uint8_t* packet=0); 929 | uint8_t dmpGetQuaternion(Quaternion *q, const uint8_t* packet=0); 930 | uint8_t dmpGet6AxisQuaternion(int32_t *data, const uint8_t* packet=0); 931 | uint8_t dmpGet6AxisQuaternion(int16_t *data, const uint8_t* packet=0); 932 | uint8_t dmpGet6AxisQuaternion(Quaternion *q, const uint8_t* packet=0); 933 | uint8_t dmpGetRelativeQuaternion(int32_t *data, const uint8_t* packet=0); 934 | uint8_t dmpGetRelativeQuaternion(int16_t *data, const uint8_t* packet=0); 935 | uint8_t dmpGetRelativeQuaternion(Quaternion *data, const uint8_t* packet=0); 936 | uint8_t dmpGetGyro(int32_t *data, const uint8_t* packet=0); 937 | uint8_t dmpGetGyro(int16_t *data, const uint8_t* packet=0); 938 | uint8_t dmpGetGyro(VectorInt16 *v, const uint8_t* packet=0); 939 | uint8_t dmpGetMag(int16_t *data, const uint8_t* packet=0); 940 | uint8_t dmpSetLinearAccelFilterCoefficient(float coef); 941 | uint8_t dmpGetLinearAccel(int32_t *data, const uint8_t* packet=0); 942 | uint8_t dmpGetLinearAccel(int16_t *data, const uint8_t* packet=0); 943 | uint8_t dmpGetLinearAccel(VectorInt16 *v, const uint8_t* packet=0); 944 | uint8_t dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity); 945 | uint8_t dmpGetLinearAccelInWorld(int32_t *data, const uint8_t* packet=0); 946 | uint8_t dmpGetLinearAccelInWorld(int16_t *data, const uint8_t* packet=0); 947 | uint8_t dmpGetLinearAccelInWorld(VectorInt16 *v, const uint8_t* packet=0); 948 | uint8_t dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q); 949 | uint8_t dmpGetGyroAndAccelSensor(int32_t *data, const uint8_t* packet=0); 950 | uint8_t dmpGetGyroAndAccelSensor(int16_t *data, const uint8_t* packet=0); 951 | uint8_t dmpGetGyroAndAccelSensor(VectorInt16 *g, VectorInt16 *a, const uint8_t* packet=0); 952 | uint8_t dmpGetGyroSensor(int32_t *data, const uint8_t* packet=0); 953 | uint8_t dmpGetGyroSensor(int16_t *data, const uint8_t* packet=0); 954 | uint8_t dmpGetGyroSensor(VectorInt16 *v, const uint8_t* packet=0); 955 | uint8_t dmpGetControlData(int32_t *data, const uint8_t* packet=0); 956 | uint8_t dmpGetTemperature(int32_t *data, const uint8_t* packet=0); 957 | uint8_t dmpGetGravity(int32_t *data, const uint8_t* packet=0); 958 | uint8_t dmpGetGravity(int16_t *data, const uint8_t* packet=0); 959 | uint8_t dmpGetGravity(VectorInt16 *v, const uint8_t* packet=0); 960 | uint8_t dmpGetGravity(VectorFloat *v, Quaternion *q); 961 | uint8_t dmpGetUnquantizedAccel(int32_t *data, const uint8_t* packet=0); 962 | uint8_t dmpGetUnquantizedAccel(int16_t *data, const uint8_t* packet=0); 963 | uint8_t dmpGetUnquantizedAccel(VectorInt16 *v, const uint8_t* packet=0); 964 | uint8_t dmpGetQuantizedAccel(int32_t *data, const uint8_t* packet=0); 965 | uint8_t dmpGetQuantizedAccel(int16_t *data, const uint8_t* packet=0); 966 | uint8_t dmpGetQuantizedAccel(VectorInt16 *v, const uint8_t* packet=0); 967 | uint8_t dmpGetExternalSensorData(int32_t *data, uint16_t size, const uint8_t* packet=0); 968 | uint8_t dmpGetEIS(int32_t *data, const uint8_t* packet=0); 969 | 970 | uint8_t dmpGetEuler(float *data, Quaternion *q); 971 | uint8_t dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity); 972 | 973 | // Get Floating Point data from FIFO 974 | uint8_t dmpGetAccelFloat(float *data, const uint8_t* packet=0); 975 | uint8_t dmpGetQuaternionFloat(float *data, const uint8_t* packet=0); 976 | 977 | uint8_t dmpProcessFIFOPacket(const unsigned char *dmpData); 978 | uint8_t dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed=NULL); 979 | 980 | uint8_t dmpSetFIFOProcessedCallback(void (*func) (void)); 981 | 982 | uint8_t dmpInitFIFOParam(); 983 | uint8_t dmpCloseFIFO(); 984 | uint8_t dmpSetGyroDataSource(uint8_t source); 985 | uint8_t dmpDecodeQuantizedAccel(); 986 | uint32_t dmpGetGyroSumOfSquare(); 987 | uint32_t dmpGetAccelSumOfSquare(); 988 | void dmpOverrideQuaternion(long *q); 989 | uint16_t dmpGetFIFOPacketSize(); 990 | #endif 991 | 992 | private: 993 | uint8_t devAddr; 994 | uint8_t buffer[14]; 995 | }; 996 | 997 | #endif /* _MPU6050_H_ */ 998 | -------------------------------------------------------------------------------- /libraries/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, 0x01 // 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 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 | -------------------------------------------------------------------------------- /libraries/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 | -------------------------------------------------------------------------------- /libraries/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_ */ -------------------------------------------------------------------------------- /libraries/PID_v1/PID_v1.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************************************** 2 | * Arduino PID Library - Version 1.0.1 3 | * by Brett Beauregard brettbeauregard.com 4 | * 5 | * This Library is licensed under a GPLv3 License 6 | **********************************************************************************************/ 7 | 8 | #if ARDUINO >= 100 9 | #include "Arduino.h" 10 | #else 11 | #include "WProgram.h" 12 | #endif 13 | 14 | #include 15 | 16 | /*Constructor (...)********************************************************* 17 | * The parameters specified here are those for for which we can't set up 18 | * reliable defaults, so we need to have the user set them. 19 | ***************************************************************************/ 20 | PID::PID(double* Input, double* Output, double* Setpoint, 21 | double Kp, double Ki, double Kd, int ControllerDirection) 22 | { 23 | PID::SetOutputLimits(0, 255); //default output limit corresponds to 24 | //the arduino pwm limits 25 | 26 | SampleTime = 100; //default Controller Sample Time is 0.1 seconds 27 | 28 | PID::SetControllerDirection(ControllerDirection); 29 | PID::SetTunings(Kp, Ki, Kd); 30 | 31 | lastTime = millis()-SampleTime; 32 | inAuto = false; 33 | myOutput = Output; 34 | myInput = Input; 35 | mySetpoint = Setpoint; 36 | 37 | } 38 | 39 | 40 | /* Compute() ********************************************************************** 41 | * This, as they say, is where the magic happens. this function should be called 42 | * every time "void loop()" executes. the function will decide for itself whether a new 43 | * pid Output needs to be computed 44 | **********************************************************************************/ 45 | void PID::Compute() 46 | { 47 | if(!inAuto) return; 48 | unsigned long now = millis(); 49 | unsigned long timeChange = (now - lastTime); 50 | if(timeChange>=SampleTime) 51 | { 52 | /*Compute all the working error variables*/ 53 | double input = *myInput; 54 | double error = *mySetpoint - input; 55 | ITerm+= (ki * error); 56 | if(ITerm > outMax) ITerm= outMax; 57 | else if(ITerm < outMin) ITerm= outMin; 58 | double dInput = (input - lastInput); 59 | 60 | /*Compute PID Output*/ 61 | double output = kp * error + ITerm- kd * dInput; 62 | 63 | if(output > outMax) output = outMax; 64 | else if(output < outMin) output = outMin; 65 | *myOutput = output; 66 | 67 | /*Remember some variables for next time*/ 68 | lastInput = input; 69 | lastTime = now; 70 | } 71 | } 72 | 73 | 74 | /* SetTunings(...)************************************************************* 75 | * This function allows the controller's dynamic performance to be adjusted. 76 | * it's called automatically from the constructor, but tunings can also 77 | * be adjusted on the fly during normal operation 78 | ******************************************************************************/ 79 | void PID::SetTunings(double Kp, double Ki, double Kd) 80 | { 81 | if (Kp<0 || Ki<0 || Kd<0) return; 82 | 83 | dispKp = Kp; dispKi = Ki; dispKd = Kd; 84 | 85 | double SampleTimeInSec = ((double)SampleTime)/1000; 86 | kp = Kp; 87 | ki = Ki * SampleTimeInSec; 88 | kd = Kd / SampleTimeInSec; 89 | 90 | if(controllerDirection ==REVERSE) 91 | { 92 | kp = (0 - kp); 93 | ki = (0 - ki); 94 | kd = (0 - kd); 95 | } 96 | } 97 | 98 | /* SetSampleTime(...) ********************************************************* 99 | * sets the period, in Milliseconds, at which the calculation is performed 100 | ******************************************************************************/ 101 | void PID::SetSampleTime(int NewSampleTime) 102 | { 103 | if (NewSampleTime > 0) 104 | { 105 | double ratio = (double)NewSampleTime 106 | / (double)SampleTime; 107 | ki *= ratio; 108 | kd /= ratio; 109 | SampleTime = (unsigned long)NewSampleTime; 110 | } 111 | } 112 | 113 | /* SetOutputLimits(...)**************************************************** 114 | * This function will be used far more often than SetInputLimits. while 115 | * the input to the controller will generally be in the 0-1023 range (which is 116 | * the default already,) the output will be a little different. maybe they'll 117 | * be doing a time window and will need 0-8000 or something. or maybe they'll 118 | * want to clamp it from 0-125. who knows. at any rate, that can all be done 119 | * here. 120 | **************************************************************************/ 121 | void PID::SetOutputLimits(double Min, double Max) 122 | { 123 | if(Min >= Max) return; 124 | outMin = Min; 125 | outMax = Max; 126 | 127 | if(inAuto) 128 | { 129 | if(*myOutput > outMax) *myOutput = outMax; 130 | else if(*myOutput < outMin) *myOutput = outMin; 131 | 132 | if(ITerm > outMax) ITerm= outMax; 133 | else if(ITerm < outMin) ITerm= outMin; 134 | } 135 | } 136 | 137 | /* SetMode(...)**************************************************************** 138 | * Allows the controller Mode to be set to manual (0) or Automatic (non-zero) 139 | * when the transition from manual to auto occurs, the controller is 140 | * automatically initialized 141 | ******************************************************************************/ 142 | void PID::SetMode(int Mode) 143 | { 144 | bool newAuto = (Mode == AUTOMATIC); 145 | if(newAuto == !inAuto) 146 | { /*we just went from manual to auto*/ 147 | PID::Initialize(); 148 | } 149 | inAuto = newAuto; 150 | } 151 | 152 | /* Initialize()**************************************************************** 153 | * does all the things that need to happen to ensure a bumpless transfer 154 | * from manual to automatic mode. 155 | ******************************************************************************/ 156 | void PID::Initialize() 157 | { 158 | ITerm = *myOutput; 159 | lastInput = *myInput; 160 | if(ITerm > outMax) ITerm = outMax; 161 | else if(ITerm < outMin) ITerm = outMin; 162 | } 163 | 164 | /* SetControllerDirection(...)************************************************* 165 | * The PID will either be connected to a DIRECT acting process (+Output leads 166 | * to +Input) or a REVERSE acting process(+Output leads to -Input.) we need to 167 | * know which one, because otherwise we may increase the output when we should 168 | * be decreasing. This is called from the constructor. 169 | ******************************************************************************/ 170 | void PID::SetControllerDirection(int Direction) 171 | { 172 | if(inAuto && Direction !=controllerDirection) 173 | { 174 | kp = (0 - kp); 175 | ki = (0 - ki); 176 | kd = (0 - kd); 177 | } 178 | controllerDirection = Direction; 179 | } 180 | 181 | /* Status Funcions************************************************************* 182 | * Just because you set the Kp=-1 doesn't mean it actually happened. these 183 | * functions query the internal state of the PID. they're here for display 184 | * purposes. this are the functions the PID Front-end uses for example 185 | ******************************************************************************/ 186 | double PID::GetKp(){ return dispKp; } 187 | double PID::GetKi(){ return dispKi;} 188 | double PID::GetKd(){ return dispKd;} 189 | int PID::GetMode(){ return inAuto ? AUTOMATIC : MANUAL;} 190 | int PID::GetDirection(){ return controllerDirection;} 191 | 192 | -------------------------------------------------------------------------------- /libraries/PID_v1/PID_v1.h: -------------------------------------------------------------------------------- 1 | #ifndef PID_v1_h 2 | #define PID_v1_h 3 | #define LIBRARY_VERSION 1.0.0 4 | 5 | class PID 6 | { 7 | 8 | 9 | public: 10 | 11 | //Constants used in some of the functions below 12 | #define AUTOMATIC 1 13 | #define MANUAL 0 14 | #define DIRECT 0 15 | #define REVERSE 1 16 | 17 | //commonly used functions ************************************************************************** 18 | PID(double*, double*, double*, // * constructor. links the PID to the Input, Output, and 19 | double, double, double, int); // Setpoint. Initial tuning parameters are also set here 20 | 21 | void SetMode(int Mode); // * sets PID to either Manual (0) or Auto (non-0) 22 | 23 | void Compute(); // * performs the PID calculation. it should be 24 | // called every time loop() cycles. ON/OFF and 25 | // calculation frequency can be set using SetMode 26 | // SetSampleTime respectively 27 | 28 | void SetOutputLimits(double, double); //clamps the output to a specific range. 0-255 by default, but 29 | //it's likely the user will want to change this depending on 30 | //the application 31 | 32 | 33 | 34 | //available but not commonly used functions ******************************************************** 35 | void SetTunings(double, double, // * While most users will set the tunings once in the 36 | double); // constructor, this function gives the user the option 37 | // of changing tunings during runtime for Adaptive control 38 | void SetControllerDirection(int); // * Sets the Direction, or "Action" of the controller. DIRECT 39 | // means the output will increase when error is positive. REVERSE 40 | // means the opposite. it's very unlikely that this will be needed 41 | // once it is set in the constructor. 42 | void SetSampleTime(int); // * sets the frequency, in Milliseconds, with which 43 | // the PID calculation is performed. default is 100 44 | 45 | 46 | 47 | //Display functions **************************************************************** 48 | double GetKp(); // These functions query the pid for interal values. 49 | double GetKi(); // they were created mainly for the pid front-end, 50 | double GetKd(); // where it's important to know what is actually 51 | int GetMode(); // inside the PID. 52 | int GetDirection(); // 53 | 54 | private: 55 | void Initialize(); 56 | 57 | double dispKp; // * we'll hold on to the tuning parameters in user-entered 58 | double dispKi; // format for display purposes 59 | double dispKd; // 60 | 61 | double kp; // * (P)roportional Tuning Parameter 62 | double ki; // * (I)ntegral Tuning Parameter 63 | double kd; // * (D)erivative Tuning Parameter 64 | 65 | int controllerDirection; 66 | 67 | double *myInput; // * Pointers to the Input, Output, and Setpoint variables 68 | double *myOutput; // This creates a hard link between the variables and the 69 | double *mySetpoint; // PID, freeing the user from having to constantly tell us 70 | // what these values are. with pointers we'll just know. 71 | 72 | unsigned long lastTime; 73 | double ITerm, lastInput; 74 | 75 | int SampleTime; 76 | double outMin, outMax; 77 | bool inAuto; 78 | }; 79 | #endif 80 | 81 | --------------------------------------------------------------------------------