├── .gitignore ├── Makefile ├── README.md ├── changelog.txt ├── lib ├── .holder ├── I2Cdev │ ├── I2Cdev.cpp │ ├── I2Cdev.h │ ├── keywords.txt │ └── library.json ├── MPU6050 │ ├── MPU6050.cpp │ ├── MPU6050.h │ ├── MPU6050_6Axis_MotionApps20.h │ ├── MPU6050_9Axis_MotionApps41.h │ ├── helper_3dmath.h │ └── library.json └── PID_v1 │ ├── PID_v1.cpp │ ├── PID_v1.h │ └── keywords.txt ├── src ├── basic_balance_motor_speed.cpp ├── basic_balance_motor_speed.h ├── constants.h ├── main.ino ├── pid_chain_motor_speed.cpp ├── pid_chain_motor_speed.h ├── pid_motor_speed.cpp ├── pid_motor_speed.h ├── pot_motor_speed.cpp ├── pot_motor_speed.h ├── printf.cpp ├── printf.h └── utils.h └── todo.txt /.gitignore: -------------------------------------------------------------------------------- 1 | .build 2 | -------------------------------------------------------------------------------- /Makefile: -------------------------------------------------------------------------------- 1 | INO=ino 2 | 3 | all: build upload serial 4 | 5 | build: 6 | $(INO) build 7 | 8 | upload: 9 | sudo $(INO) upload 10 | 11 | serial: 12 | sudo $(INO) serial -b 115200 13 | 14 | clean: 15 | $(INO) clean 16 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Self Balancing Robot Arduino sketch 2 | 3 | **Note: This readme is a work in progress - the project is still under development** 4 | 5 | Sketch for a self balancing arduino robot using an [Arduino Uno](http://www.dx.com/pt/p/uno-r3-atmega328p-development-board-for-arduino-402904?Utm_rid=60225380&Utm_source=affiliate), a [MPU6050](http://www.dx.com/p/gy-521-mpu6050-3-axis-acceleration-gyroscope-6dof-module-blue-154602?Utm_rid=60225380&Utm_source=affiliate), [NEMA 17 motors](http://www.dx.com/pt/p/geeetech-1-8-degree-nema-14-35-byghw-stepper-motor-for-3d-printer-black-386069?Utm_rid=60225380&Utm_source=affiliate) and two [A4988](http://www.dx.com/pt/p/3d-printer-a4988-arduino-reprap-stepper-motor-driver-265980?Utm_rid=60225380&Utm_source=affiliate) drivers. 6 | 7 | ## Wiring 8 | 9 | #### MPU6050 wiring 10 | * GND/VCC shared with the rest of the circuit - perhaps change GND to second arduino gnd. 11 | * SDA/SCL to A4/A5 respectivly 12 | 13 | #### NEMA 17 wiring 14 | [NEMA 17 motors](http://www.dx.com/pt/p/geeetech-1-8-degree-nema-14-35-byghw-stepper-motor-for-3d-printer-black-386069?Utm_rid=60225380&Utm_source=affiliate) with 4 wires scheme: 15 |

16 | 17 |

30 | 31 |

32 | 33 | Current wiring of the A4988 and Arduino - motor wires ***not*** connected for image readability sake. 34 | * Orange and Yellow wires are step and dir (respectively) 35 | * All the MS are connected to Arduino 5v 36 | 37 |

38 | 39 |

40 | 41 | ## Current state of the robot 42 | * Current state as of: 22/01/2016 - youtube video [here](https://www.youtube.com/watch?v=yMFi8TMg03o) 43 | * Current state as of: 03/02/2016 - youtube video [here](https://www.youtube.com/watch?v=o339cVn5oNA) 44 | 45 | 46 |

47 | 48 |

49 | -------------------------------------------------------------------------------- /changelog.txt: -------------------------------------------------------------------------------- 1 | Dev - 09/01/16 2 | * changed scl to run at 400khz for faster mpu readings 3 | * changed speed period formula to (y=-0.017x+10.5) 4 | * changed timer1 implementation to use arduino constants instead of lib - runs 33khz 5 | * added prints with tag functionality 6 | * added runEvery macro to simplify debug information 7 | * changed all the debug information to use the new macro 8 | 9 | Dev - 08/01/16 10 | * removed period calculation out of the callback function, was taking too long and stalling the program 11 | * changed the modules function to calculate acceleration instead of velocity 12 | * changed the callback function to account for 2 independent motor velocities 13 | * remove va_args from module functions - was not working properly 14 | 15 | Dev - 07/01/16 16 | * added basic balancing module without pid logic - test purposes 17 | * added printf support - sick of Serial.println shit 18 | * added multi-module support 19 | -------------------------------------------------------------------------------- /lib/.holder: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jcfs/self-balancing-robot/9beba1f8dd3dacef894e4145827a6e4ef7b176e6/lib/.holder -------------------------------------------------------------------------------- /lib/I2Cdev/I2Cdev.h: -------------------------------------------------------------------------------- 1 | // I2Cdev library collection - Main I2C device class header file 2 | // Abstracts bit and byte I2C R/W functions into a convenient class 3 | // 2013-06-05 by Jeff Rowberg 4 | // 5 | // Changelog: 6 | // 2013-05-06 - add Francesco Ferrara's Fastwire v0.24 implementation with small modifications 7 | // 2013-05-05 - fix issue with writing bit values to words (Sasquatch/Farzanegan) 8 | // 2012-06-09 - fix major issue with reading > 32 bytes at a time with Arduino Wire 9 | // - add compiler warnings when using outdated or IDE or limited I2Cdev implementation 10 | // 2011-11-01 - fix write*Bits mask calculation (thanks sasquatch @ Arduino forums) 11 | // 2011-10-03 - added automatic Arduino version detection for ease of use 12 | // 2011-10-02 - added Gene Knight's NBWire TwoWire class implementation with small modifications 13 | // 2011-08-31 - added support for Arduino 1.0 Wire library (methods are different from 0.x) 14 | // 2011-08-03 - added optional timeout parameter to read* methods to easily change from default 15 | // 2011-08-02 - added support for 16-bit registers 16 | // - fixed incorrect Doxygen comments on some methods 17 | // - added timeout value for read operations (thanks mem @ Arduino forums) 18 | // 2011-07-30 - changed read/write function structures to return success or byte counts 19 | // - made all methods static for multi-device memory savings 20 | // 2011-07-28 - initial release 21 | 22 | /* ============================================ 23 | I2Cdev device library code is placed under the MIT license 24 | Copyright (c) 2013 Jeff Rowberg 25 | 26 | Permission is hereby granted, free of charge, to any person obtaining a copy 27 | of this software and associated documentation files (the "Software"), to deal 28 | in the Software without restriction, including without limitation the rights 29 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 30 | copies of the Software, and to permit persons to whom the Software is 31 | furnished to do so, subject to the following conditions: 32 | 33 | The above copyright notice and this permission notice shall be included in 34 | all copies or substantial portions of the Software. 35 | 36 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 37 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 38 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 39 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 40 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 41 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 42 | THE SOFTWARE. 43 | =============================================== 44 | */ 45 | 46 | #ifndef _I2CDEV_H_ 47 | #define _I2CDEV_H_ 48 | 49 | // ----------------------------------------------------------------------------- 50 | // I2C interface implementation setting 51 | // ----------------------------------------------------------------------------- 52 | #ifndef I2CDEV_IMPLEMENTATION 53 | #define I2CDEV_IMPLEMENTATION I2CDEV_ARDUINO_WIRE 54 | //#define I2CDEV_IMPLEMENTATION I2CDEV_BUILTIN_FASTWIRE 55 | #endif // I2CDEV_IMPLEMENTATION 56 | 57 | // comment this out if you are using a non-optimal IDE/implementation setting 58 | // but want the compiler to shut up about it 59 | #define I2CDEV_IMPLEMENTATION_WARNINGS 60 | 61 | // ----------------------------------------------------------------------------- 62 | // I2C interface implementation options 63 | // ----------------------------------------------------------------------------- 64 | #define I2CDEV_ARDUINO_WIRE 1 // Wire object from Arduino 65 | #define I2CDEV_BUILTIN_NBWIRE 2 // Tweaked Wire object from Gene Knight's NBWire project 66 | // ^^^ NBWire implementation is still buggy w/some interrupts! 67 | #define I2CDEV_BUILTIN_FASTWIRE 3 // FastWire object from Francesco Ferrara's project 68 | #define I2CDEV_I2CMASTER_LIBRARY 4 // I2C object from DSSCircuits I2C-Master Library at https://github.com/DSSCircuits/I2C-Master-Library 69 | 70 | // ----------------------------------------------------------------------------- 71 | // Arduino-style "Serial.print" debug constant (uncomment to enable) 72 | // ----------------------------------------------------------------------------- 73 | //#define I2CDEV_SERIAL_DEBUG 74 | 75 | #ifdef ARDUINO 76 | #if ARDUINO < 100 77 | #include "WProgram.h" 78 | #else 79 | #include "Arduino.h" 80 | #endif 81 | #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE 82 | #include 83 | #endif 84 | #if I2CDEV_IMPLEMENTATION == I2CDEV_I2CMASTER_LIBRARY 85 | #include 86 | #endif 87 | #endif 88 | 89 | #ifdef SPARK 90 | #include 91 | #define ARDUINO 101 92 | #endif 93 | 94 | // 1000ms default read timeout (modify with "I2Cdev::readTimeout = [ms];") 95 | #define I2CDEV_DEFAULT_READ_TIMEOUT 1000 96 | 97 | class I2Cdev { 98 | public: 99 | I2Cdev(); 100 | 101 | static int8_t readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout); 102 | static int8_t readBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout); 103 | static int8_t readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout); 104 | static int8_t readBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout); 105 | static int8_t readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout); 106 | static int8_t readWord(uint8_t devAddr, uint8_t regAddr, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout); 107 | static int8_t readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout); 108 | static int8_t readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout); 109 | 110 | static bool writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data); 111 | static bool writeBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t data); 112 | static bool writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data); 113 | static bool writeBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t data); 114 | static bool writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data); 115 | static bool writeWord(uint8_t devAddr, uint8_t regAddr, uint16_t data); 116 | static bool writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data); 117 | static bool writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data); 118 | 119 | static uint16_t readTimeout; 120 | }; 121 | 122 | #if I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE 123 | ////////////////////// 124 | // FastWire 0.24 125 | // This is a library to help faster programs to read I2C devices. 126 | // Copyright(C) 2012 127 | // Francesco Ferrara 128 | ////////////////////// 129 | 130 | /* Master */ 131 | #define TW_START 0x08 132 | #define TW_REP_START 0x10 133 | 134 | /* Master Transmitter */ 135 | #define TW_MT_SLA_ACK 0x18 136 | #define TW_MT_SLA_NACK 0x20 137 | #define TW_MT_DATA_ACK 0x28 138 | #define TW_MT_DATA_NACK 0x30 139 | #define TW_MT_ARB_LOST 0x38 140 | 141 | /* Master Receiver */ 142 | #define TW_MR_ARB_LOST 0x38 143 | #define TW_MR_SLA_ACK 0x40 144 | #define TW_MR_SLA_NACK 0x48 145 | #define TW_MR_DATA_ACK 0x50 146 | #define TW_MR_DATA_NACK 0x58 147 | 148 | #define TW_OK 0 149 | #define TW_ERROR 1 150 | 151 | class Fastwire { 152 | private: 153 | static boolean waitInt(); 154 | 155 | public: 156 | static void setup(int khz, boolean pullup); 157 | static byte beginTransmission(byte device); 158 | static byte write(byte value); 159 | static byte writeBuf(byte device, byte address, byte *data, byte num); 160 | static byte readBuf(byte device, byte address, byte *data, byte num); 161 | static void reset(); 162 | static byte stop(); 163 | }; 164 | #endif 165 | 166 | #if I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE 167 | // NBWire implementation based heavily on code by Gene Knight 168 | // Originally posted on the Arduino forum at http://arduino.cc/forum/index.php/topic,70705.0.html 169 | // Originally offered to the i2cdevlib project at http://arduino.cc/forum/index.php/topic,68210.30.html 170 | 171 | #define NBWIRE_BUFFER_LENGTH 32 172 | 173 | class TwoWire { 174 | private: 175 | static uint8_t rxBuffer[]; 176 | static uint8_t rxBufferIndex; 177 | static uint8_t rxBufferLength; 178 | 179 | static uint8_t txAddress; 180 | static uint8_t txBuffer[]; 181 | static uint8_t txBufferIndex; 182 | static uint8_t txBufferLength; 183 | 184 | // static uint8_t transmitting; 185 | static void (*user_onRequest)(void); 186 | static void (*user_onReceive)(int); 187 | static void onRequestService(void); 188 | static void onReceiveService(uint8_t*, int); 189 | 190 | public: 191 | TwoWire(); 192 | void begin(); 193 | void begin(uint8_t); 194 | void begin(int); 195 | void beginTransmission(uint8_t); 196 | //void beginTransmission(int); 197 | uint8_t endTransmission(uint16_t timeout=0); 198 | void nbendTransmission(void (*function)(int)) ; 199 | uint8_t requestFrom(uint8_t, int, uint16_t timeout=0); 200 | //uint8_t requestFrom(int, int); 201 | void nbrequestFrom(uint8_t, int, void (*function)(int)); 202 | void send(uint8_t); 203 | void send(uint8_t*, uint8_t); 204 | //void send(int); 205 | void send(char*); 206 | uint8_t available(void); 207 | uint8_t receive(void); 208 | void onReceive(void (*)(int)); 209 | void onRequest(void (*)(void)); 210 | }; 211 | 212 | #define TWI_READY 0 213 | #define TWI_MRX 1 214 | #define TWI_MTX 2 215 | #define TWI_SRX 3 216 | #define TWI_STX 4 217 | 218 | #define TW_WRITE 0 219 | #define TW_READ 1 220 | 221 | #define TW_MT_SLA_NACK 0x20 222 | #define TW_MT_DATA_NACK 0x30 223 | 224 | #define CPU_FREQ 16000000L 225 | #define TWI_FREQ 100000L 226 | #define TWI_BUFFER_LENGTH 32 227 | 228 | /* TWI Status is in TWSR, in the top 5 bits: TWS7 - TWS3 */ 229 | 230 | #define TW_STATUS_MASK (_BV(TWS7)|_BV(TWS6)|_BV(TWS5)|_BV(TWS4)|_BV(TWS3)) 231 | #define TW_STATUS (TWSR & TW_STATUS_MASK) 232 | #define TW_START 0x08 233 | #define TW_REP_START 0x10 234 | #define TW_MT_SLA_ACK 0x18 235 | #define TW_MT_SLA_NACK 0x20 236 | #define TW_MT_DATA_ACK 0x28 237 | #define TW_MT_DATA_NACK 0x30 238 | #define TW_MT_ARB_LOST 0x38 239 | #define TW_MR_ARB_LOST 0x38 240 | #define TW_MR_SLA_ACK 0x40 241 | #define TW_MR_SLA_NACK 0x48 242 | #define TW_MR_DATA_ACK 0x50 243 | #define TW_MR_DATA_NACK 0x58 244 | #define TW_ST_SLA_ACK 0xA8 245 | #define TW_ST_ARB_LOST_SLA_ACK 0xB0 246 | #define TW_ST_DATA_ACK 0xB8 247 | #define TW_ST_DATA_NACK 0xC0 248 | #define TW_ST_LAST_DATA 0xC8 249 | #define TW_SR_SLA_ACK 0x60 250 | #define TW_SR_ARB_LOST_SLA_ACK 0x68 251 | #define TW_SR_GCALL_ACK 0x70 252 | #define TW_SR_ARB_LOST_GCALL_ACK 0x78 253 | #define TW_SR_DATA_ACK 0x80 254 | #define TW_SR_DATA_NACK 0x88 255 | #define TW_SR_GCALL_DATA_ACK 0x90 256 | #define TW_SR_GCALL_DATA_NACK 0x98 257 | #define TW_SR_STOP 0xA0 258 | #define TW_NO_INFO 0xF8 259 | #define TW_BUS_ERROR 0x00 260 | 261 | //#define _MMIO_BYTE(mem_addr) (*(volatile uint8_t *)(mem_addr)) 262 | //#define _SFR_BYTE(sfr) _MMIO_BYTE(_SFR_ADDR(sfr)) 263 | 264 | #ifndef sbi // set bit 265 | #define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit)) 266 | #endif // sbi 267 | 268 | #ifndef cbi // clear bit 269 | #define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit)) 270 | #endif // cbi 271 | 272 | extern TwoWire Wire; 273 | 274 | #endif // I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE 275 | 276 | #endif /* _I2CDEV_H_ */ 277 | -------------------------------------------------------------------------------- /lib/I2Cdev/keywords.txt: -------------------------------------------------------------------------------- 1 | ####################################### 2 | # Syntax Coloring Map For I2Cdev 3 | ####################################### 4 | 5 | ####################################### 6 | # Datatypes (KEYWORD1) 7 | ####################################### 8 | I2Cdev KEYWORD1 9 | 10 | ####################################### 11 | # Methods and Functions (KEYWORD2) 12 | ####################################### 13 | 14 | readBit KEYWORD2 15 | readBitW KEYWORD2 16 | readBits KEYWORD2 17 | readBitsW KEYWORD2 18 | readByte KEYWORD2 19 | readBytes KEYWORD2 20 | readWord KEYWORD2 21 | readWords KEYWORD2 22 | writeBit KEYWORD2 23 | writeBitW KEYWORD2 24 | writeBits KEYWORD2 25 | writeBitsW KEYWORD2 26 | writeByte KEYWORD2 27 | writeBytes KEYWORD2 28 | writeWord KEYWORD2 29 | writeWords KEYWORD2 30 | 31 | ####################################### 32 | # Instances (KEYWORD2) 33 | ####################################### 34 | 35 | ####################################### 36 | # Constants (LITERAL1) 37 | ####################################### 38 | 39 | -------------------------------------------------------------------------------- /lib/I2Cdev/library.json: -------------------------------------------------------------------------------- 1 | { 2 | "name": "I2Cdevlib-Core", 3 | "keywords": "i2cdevlib, i2c", 4 | "description": "The I2C Device Library (I2Cdevlib) is a collection of uniform and well-documented classes to provide simple and intuitive interfaces to I2C devices.", 5 | "include": "Arduino/I2Cdev", 6 | "repository": 7 | { 8 | "type": "git", 9 | "url": "https://github.com/jrowberg/i2cdevlib.git" 10 | }, 11 | "frameworks": "arduino", 12 | "platforms": "atmelavr" 13 | } 14 | -------------------------------------------------------------------------------- /lib/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 | 45 | #ifndef __arm__ 46 | #include 47 | #else 48 | //#define PROGMEM /* empty */ 49 | //#define pgm_read_byte(x) (*(x)) 50 | //#define pgm_read_word(x) (*(x)) 51 | //#define pgm_read_float(x) (*(x)) 52 | //#define PSTR(STR) STR 53 | #endif 54 | 55 | 56 | #define MPU6050_ADDRESS_AD0_LOW 0x68 // address pin low (GND), default for InvenSense evaluation board 57 | #define MPU6050_ADDRESS_AD0_HIGH 0x69 // address pin high (VCC) 58 | #define MPU6050_DEFAULT_ADDRESS MPU6050_ADDRESS_AD0_LOW 59 | 60 | #define MPU6050_RA_XG_OFFS_TC 0x00 //[7] PWR_MODE, [6:1] XG_OFFS_TC, [0] OTP_BNK_VLD 61 | #define MPU6050_RA_YG_OFFS_TC 0x01 //[7] PWR_MODE, [6:1] YG_OFFS_TC, [0] OTP_BNK_VLD 62 | #define MPU6050_RA_ZG_OFFS_TC 0x02 //[7] PWR_MODE, [6:1] ZG_OFFS_TC, [0] OTP_BNK_VLD 63 | #define MPU6050_RA_X_FINE_GAIN 0x03 //[7:0] X_FINE_GAIN 64 | #define MPU6050_RA_Y_FINE_GAIN 0x04 //[7:0] Y_FINE_GAIN 65 | #define MPU6050_RA_Z_FINE_GAIN 0x05 //[7:0] Z_FINE_GAIN 66 | #define MPU6050_RA_XA_OFFS_H 0x06 //[15:0] XA_OFFS 67 | #define MPU6050_RA_XA_OFFS_L_TC 0x07 68 | #define MPU6050_RA_YA_OFFS_H 0x08 //[15:0] YA_OFFS 69 | #define MPU6050_RA_YA_OFFS_L_TC 0x09 70 | #define MPU6050_RA_ZA_OFFS_H 0x0A //[15:0] ZA_OFFS 71 | #define MPU6050_RA_ZA_OFFS_L_TC 0x0B 72 | #define MPU6050_RA_SELF_TEST_X 0x0D //[7:5] XA_TEST[4-2], [4:0] XG_TEST[4-0] 73 | #define MPU6050_RA_SELF_TEST_Y 0x0E //[7:5] YA_TEST[4-2], [4:0] YG_TEST[4-0] 74 | #define MPU6050_RA_SELF_TEST_Z 0x0F //[7:5] ZA_TEST[4-2], [4:0] ZG_TEST[4-0] 75 | #define MPU6050_RA_SELF_TEST_A 0x10 //[5:4] XA_TEST[1-0], [3:2] YA_TEST[1-0], [1:0] ZA_TEST[1-0] 76 | #define MPU6050_RA_XG_OFFS_USRH 0x13 //[15:0] XG_OFFS_USR 77 | #define MPU6050_RA_XG_OFFS_USRL 0x14 78 | #define MPU6050_RA_YG_OFFS_USRH 0x15 //[15:0] YG_OFFS_USR 79 | #define MPU6050_RA_YG_OFFS_USRL 0x16 80 | #define MPU6050_RA_ZG_OFFS_USRH 0x17 //[15:0] ZG_OFFS_USR 81 | #define MPU6050_RA_ZG_OFFS_USRL 0x18 82 | #define MPU6050_RA_SMPLRT_DIV 0x19 83 | #define MPU6050_RA_CONFIG 0x1A 84 | #define MPU6050_RA_GYRO_CONFIG 0x1B 85 | #define MPU6050_RA_ACCEL_CONFIG 0x1C 86 | #define MPU6050_RA_FF_THR 0x1D 87 | #define MPU6050_RA_FF_DUR 0x1E 88 | #define MPU6050_RA_MOT_THR 0x1F 89 | #define MPU6050_RA_MOT_DUR 0x20 90 | #define MPU6050_RA_ZRMOT_THR 0x21 91 | #define MPU6050_RA_ZRMOT_DUR 0x22 92 | #define MPU6050_RA_FIFO_EN 0x23 93 | #define MPU6050_RA_I2C_MST_CTRL 0x24 94 | #define MPU6050_RA_I2C_SLV0_ADDR 0x25 95 | #define MPU6050_RA_I2C_SLV0_REG 0x26 96 | #define MPU6050_RA_I2C_SLV0_CTRL 0x27 97 | #define MPU6050_RA_I2C_SLV1_ADDR 0x28 98 | #define MPU6050_RA_I2C_SLV1_REG 0x29 99 | #define MPU6050_RA_I2C_SLV1_CTRL 0x2A 100 | #define MPU6050_RA_I2C_SLV2_ADDR 0x2B 101 | #define MPU6050_RA_I2C_SLV2_REG 0x2C 102 | #define MPU6050_RA_I2C_SLV2_CTRL 0x2D 103 | #define MPU6050_RA_I2C_SLV3_ADDR 0x2E 104 | #define MPU6050_RA_I2C_SLV3_REG 0x2F 105 | #define MPU6050_RA_I2C_SLV3_CTRL 0x30 106 | #define MPU6050_RA_I2C_SLV4_ADDR 0x31 107 | #define MPU6050_RA_I2C_SLV4_REG 0x32 108 | #define MPU6050_RA_I2C_SLV4_DO 0x33 109 | #define MPU6050_RA_I2C_SLV4_CTRL 0x34 110 | #define MPU6050_RA_I2C_SLV4_DI 0x35 111 | #define MPU6050_RA_I2C_MST_STATUS 0x36 112 | #define MPU6050_RA_INT_PIN_CFG 0x37 113 | #define MPU6050_RA_INT_ENABLE 0x38 114 | #define MPU6050_RA_DMP_INT_STATUS 0x39 115 | #define MPU6050_RA_INT_STATUS 0x3A 116 | #define MPU6050_RA_ACCEL_XOUT_H 0x3B 117 | #define MPU6050_RA_ACCEL_XOUT_L 0x3C 118 | #define MPU6050_RA_ACCEL_YOUT_H 0x3D 119 | #define MPU6050_RA_ACCEL_YOUT_L 0x3E 120 | #define MPU6050_RA_ACCEL_ZOUT_H 0x3F 121 | #define MPU6050_RA_ACCEL_ZOUT_L 0x40 122 | #define MPU6050_RA_TEMP_OUT_H 0x41 123 | #define MPU6050_RA_TEMP_OUT_L 0x42 124 | #define MPU6050_RA_GYRO_XOUT_H 0x43 125 | #define MPU6050_RA_GYRO_XOUT_L 0x44 126 | #define MPU6050_RA_GYRO_YOUT_H 0x45 127 | #define MPU6050_RA_GYRO_YOUT_L 0x46 128 | #define MPU6050_RA_GYRO_ZOUT_H 0x47 129 | #define MPU6050_RA_GYRO_ZOUT_L 0x48 130 | #define MPU6050_RA_EXT_SENS_DATA_00 0x49 131 | #define MPU6050_RA_EXT_SENS_DATA_01 0x4A 132 | #define MPU6050_RA_EXT_SENS_DATA_02 0x4B 133 | #define MPU6050_RA_EXT_SENS_DATA_03 0x4C 134 | #define MPU6050_RA_EXT_SENS_DATA_04 0x4D 135 | #define MPU6050_RA_EXT_SENS_DATA_05 0x4E 136 | #define MPU6050_RA_EXT_SENS_DATA_06 0x4F 137 | #define MPU6050_RA_EXT_SENS_DATA_07 0x50 138 | #define MPU6050_RA_EXT_SENS_DATA_08 0x51 139 | #define MPU6050_RA_EXT_SENS_DATA_09 0x52 140 | #define MPU6050_RA_EXT_SENS_DATA_10 0x53 141 | #define MPU6050_RA_EXT_SENS_DATA_11 0x54 142 | #define MPU6050_RA_EXT_SENS_DATA_12 0x55 143 | #define MPU6050_RA_EXT_SENS_DATA_13 0x56 144 | #define MPU6050_RA_EXT_SENS_DATA_14 0x57 145 | #define MPU6050_RA_EXT_SENS_DATA_15 0x58 146 | #define MPU6050_RA_EXT_SENS_DATA_16 0x59 147 | #define MPU6050_RA_EXT_SENS_DATA_17 0x5A 148 | #define MPU6050_RA_EXT_SENS_DATA_18 0x5B 149 | #define MPU6050_RA_EXT_SENS_DATA_19 0x5C 150 | #define MPU6050_RA_EXT_SENS_DATA_20 0x5D 151 | #define MPU6050_RA_EXT_SENS_DATA_21 0x5E 152 | #define MPU6050_RA_EXT_SENS_DATA_22 0x5F 153 | #define MPU6050_RA_EXT_SENS_DATA_23 0x60 154 | #define MPU6050_RA_MOT_DETECT_STATUS 0x61 155 | #define MPU6050_RA_I2C_SLV0_DO 0x63 156 | #define MPU6050_RA_I2C_SLV1_DO 0x64 157 | #define MPU6050_RA_I2C_SLV2_DO 0x65 158 | #define MPU6050_RA_I2C_SLV3_DO 0x66 159 | #define MPU6050_RA_I2C_MST_DELAY_CTRL 0x67 160 | #define MPU6050_RA_SIGNAL_PATH_RESET 0x68 161 | #define MPU6050_RA_MOT_DETECT_CTRL 0x69 162 | #define MPU6050_RA_USER_CTRL 0x6A 163 | #define MPU6050_RA_PWR_MGMT_1 0x6B 164 | #define MPU6050_RA_PWR_MGMT_2 0x6C 165 | #define MPU6050_RA_BANK_SEL 0x6D 166 | #define MPU6050_RA_MEM_START_ADDR 0x6E 167 | #define MPU6050_RA_MEM_R_W 0x6F 168 | #define MPU6050_RA_DMP_CFG_1 0x70 169 | #define MPU6050_RA_DMP_CFG_2 0x71 170 | #define MPU6050_RA_FIFO_COUNTH 0x72 171 | #define MPU6050_RA_FIFO_COUNTL 0x73 172 | #define MPU6050_RA_FIFO_R_W 0x74 173 | #define MPU6050_RA_WHO_AM_I 0x75 174 | 175 | #define MPU6050_SELF_TEST_XA_1_BIT 0x07 176 | #define MPU6050_SELF_TEST_XA_1_LENGTH 0x03 177 | #define MPU6050_SELF_TEST_XA_2_BIT 0x05 178 | #define MPU6050_SELF_TEST_XA_2_LENGTH 0x02 179 | #define MPU6050_SELF_TEST_YA_1_BIT 0x07 180 | #define MPU6050_SELF_TEST_YA_1_LENGTH 0x03 181 | #define MPU6050_SELF_TEST_YA_2_BIT 0x03 182 | #define MPU6050_SELF_TEST_YA_2_LENGTH 0x02 183 | #define MPU6050_SELF_TEST_ZA_1_BIT 0x07 184 | #define MPU6050_SELF_TEST_ZA_1_LENGTH 0x03 185 | #define MPU6050_SELF_TEST_ZA_2_BIT 0x01 186 | #define MPU6050_SELF_TEST_ZA_2_LENGTH 0x02 187 | 188 | #define MPU6050_SELF_TEST_XG_1_BIT 0x04 189 | #define MPU6050_SELF_TEST_XG_1_LENGTH 0x05 190 | #define MPU6050_SELF_TEST_YG_1_BIT 0x04 191 | #define MPU6050_SELF_TEST_YG_1_LENGTH 0x05 192 | #define MPU6050_SELF_TEST_ZG_1_BIT 0x04 193 | #define MPU6050_SELF_TEST_ZG_1_LENGTH 0x05 194 | 195 | #define MPU6050_TC_PWR_MODE_BIT 7 196 | #define MPU6050_TC_OFFSET_BIT 6 197 | #define MPU6050_TC_OFFSET_LENGTH 6 198 | #define MPU6050_TC_OTP_BNK_VLD_BIT 0 199 | 200 | #define MPU6050_VDDIO_LEVEL_VLOGIC 0 201 | #define MPU6050_VDDIO_LEVEL_VDD 1 202 | 203 | #define MPU6050_CFG_EXT_SYNC_SET_BIT 5 204 | #define MPU6050_CFG_EXT_SYNC_SET_LENGTH 3 205 | #define MPU6050_CFG_DLPF_CFG_BIT 2 206 | #define MPU6050_CFG_DLPF_CFG_LENGTH 3 207 | 208 | #define MPU6050_EXT_SYNC_DISABLED 0x0 209 | #define MPU6050_EXT_SYNC_TEMP_OUT_L 0x1 210 | #define MPU6050_EXT_SYNC_GYRO_XOUT_L 0x2 211 | #define MPU6050_EXT_SYNC_GYRO_YOUT_L 0x3 212 | #define MPU6050_EXT_SYNC_GYRO_ZOUT_L 0x4 213 | #define MPU6050_EXT_SYNC_ACCEL_XOUT_L 0x5 214 | #define MPU6050_EXT_SYNC_ACCEL_YOUT_L 0x6 215 | #define MPU6050_EXT_SYNC_ACCEL_ZOUT_L 0x7 216 | 217 | #define MPU6050_DLPF_BW_256 0x00 218 | #define MPU6050_DLPF_BW_188 0x01 219 | #define MPU6050_DLPF_BW_98 0x02 220 | #define MPU6050_DLPF_BW_42 0x03 221 | #define MPU6050_DLPF_BW_20 0x04 222 | #define MPU6050_DLPF_BW_10 0x05 223 | #define MPU6050_DLPF_BW_5 0x06 224 | 225 | #define MPU6050_GCONFIG_FS_SEL_BIT 4 226 | #define MPU6050_GCONFIG_FS_SEL_LENGTH 2 227 | 228 | #define MPU6050_GYRO_FS_250 0x00 229 | #define MPU6050_GYRO_FS_500 0x01 230 | #define MPU6050_GYRO_FS_1000 0x02 231 | #define MPU6050_GYRO_FS_2000 0x03 232 | 233 | #define MPU6050_ACONFIG_XA_ST_BIT 7 234 | #define MPU6050_ACONFIG_YA_ST_BIT 6 235 | #define MPU6050_ACONFIG_ZA_ST_BIT 5 236 | #define MPU6050_ACONFIG_AFS_SEL_BIT 4 237 | #define MPU6050_ACONFIG_AFS_SEL_LENGTH 2 238 | #define MPU6050_ACONFIG_ACCEL_HPF_BIT 2 239 | #define MPU6050_ACONFIG_ACCEL_HPF_LENGTH 3 240 | 241 | #define MPU6050_ACCEL_FS_2 0x00 242 | #define MPU6050_ACCEL_FS_4 0x01 243 | #define MPU6050_ACCEL_FS_8 0x02 244 | #define MPU6050_ACCEL_FS_16 0x03 245 | 246 | #define MPU6050_DHPF_RESET 0x00 247 | #define MPU6050_DHPF_5 0x01 248 | #define MPU6050_DHPF_2P5 0x02 249 | #define MPU6050_DHPF_1P25 0x03 250 | #define MPU6050_DHPF_0P63 0x04 251 | #define MPU6050_DHPF_HOLD 0x07 252 | 253 | #define MPU6050_TEMP_FIFO_EN_BIT 7 254 | #define MPU6050_XG_FIFO_EN_BIT 6 255 | #define MPU6050_YG_FIFO_EN_BIT 5 256 | #define MPU6050_ZG_FIFO_EN_BIT 4 257 | #define MPU6050_ACCEL_FIFO_EN_BIT 3 258 | #define MPU6050_SLV2_FIFO_EN_BIT 2 259 | #define MPU6050_SLV1_FIFO_EN_BIT 1 260 | #define MPU6050_SLV0_FIFO_EN_BIT 0 261 | 262 | #define MPU6050_MULT_MST_EN_BIT 7 263 | #define MPU6050_WAIT_FOR_ES_BIT 6 264 | #define MPU6050_SLV_3_FIFO_EN_BIT 5 265 | #define MPU6050_I2C_MST_P_NSR_BIT 4 266 | #define MPU6050_I2C_MST_CLK_BIT 3 267 | #define MPU6050_I2C_MST_CLK_LENGTH 4 268 | 269 | #define MPU6050_CLOCK_DIV_348 0x0 270 | #define MPU6050_CLOCK_DIV_333 0x1 271 | #define MPU6050_CLOCK_DIV_320 0x2 272 | #define MPU6050_CLOCK_DIV_308 0x3 273 | #define MPU6050_CLOCK_DIV_296 0x4 274 | #define MPU6050_CLOCK_DIV_286 0x5 275 | #define MPU6050_CLOCK_DIV_276 0x6 276 | #define MPU6050_CLOCK_DIV_267 0x7 277 | #define MPU6050_CLOCK_DIV_258 0x8 278 | #define MPU6050_CLOCK_DIV_500 0x9 279 | #define MPU6050_CLOCK_DIV_471 0xA 280 | #define MPU6050_CLOCK_DIV_444 0xB 281 | #define MPU6050_CLOCK_DIV_421 0xC 282 | #define MPU6050_CLOCK_DIV_400 0xD 283 | #define MPU6050_CLOCK_DIV_381 0xE 284 | #define MPU6050_CLOCK_DIV_364 0xF 285 | 286 | #define MPU6050_I2C_SLV_RW_BIT 7 287 | #define MPU6050_I2C_SLV_ADDR_BIT 6 288 | #define MPU6050_I2C_SLV_ADDR_LENGTH 7 289 | #define MPU6050_I2C_SLV_EN_BIT 7 290 | #define MPU6050_I2C_SLV_BYTE_SW_BIT 6 291 | #define MPU6050_I2C_SLV_REG_DIS_BIT 5 292 | #define MPU6050_I2C_SLV_GRP_BIT 4 293 | #define MPU6050_I2C_SLV_LEN_BIT 3 294 | #define MPU6050_I2C_SLV_LEN_LENGTH 4 295 | 296 | #define MPU6050_I2C_SLV4_RW_BIT 7 297 | #define MPU6050_I2C_SLV4_ADDR_BIT 6 298 | #define MPU6050_I2C_SLV4_ADDR_LENGTH 7 299 | #define MPU6050_I2C_SLV4_EN_BIT 7 300 | #define MPU6050_I2C_SLV4_INT_EN_BIT 6 301 | #define MPU6050_I2C_SLV4_REG_DIS_BIT 5 302 | #define MPU6050_I2C_SLV4_MST_DLY_BIT 4 303 | #define MPU6050_I2C_SLV4_MST_DLY_LENGTH 5 304 | 305 | #define MPU6050_MST_PASS_THROUGH_BIT 7 306 | #define MPU6050_MST_I2C_SLV4_DONE_BIT 6 307 | #define MPU6050_MST_I2C_LOST_ARB_BIT 5 308 | #define MPU6050_MST_I2C_SLV4_NACK_BIT 4 309 | #define MPU6050_MST_I2C_SLV3_NACK_BIT 3 310 | #define MPU6050_MST_I2C_SLV2_NACK_BIT 2 311 | #define MPU6050_MST_I2C_SLV1_NACK_BIT 1 312 | #define MPU6050_MST_I2C_SLV0_NACK_BIT 0 313 | 314 | #define MPU6050_INTCFG_INT_LEVEL_BIT 7 315 | #define MPU6050_INTCFG_INT_OPEN_BIT 6 316 | #define MPU6050_INTCFG_LATCH_INT_EN_BIT 5 317 | #define MPU6050_INTCFG_INT_RD_CLEAR_BIT 4 318 | #define MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT 3 319 | #define MPU6050_INTCFG_FSYNC_INT_EN_BIT 2 320 | #define MPU6050_INTCFG_I2C_BYPASS_EN_BIT 1 321 | #define MPU6050_INTCFG_CLKOUT_EN_BIT 0 322 | 323 | #define MPU6050_INTMODE_ACTIVEHIGH 0x00 324 | #define MPU6050_INTMODE_ACTIVELOW 0x01 325 | 326 | #define MPU6050_INTDRV_PUSHPULL 0x00 327 | #define MPU6050_INTDRV_OPENDRAIN 0x01 328 | 329 | #define MPU6050_INTLATCH_50USPULSE 0x00 330 | #define MPU6050_INTLATCH_WAITCLEAR 0x01 331 | 332 | #define MPU6050_INTCLEAR_STATUSREAD 0x00 333 | #define MPU6050_INTCLEAR_ANYREAD 0x01 334 | 335 | #define MPU6050_INTERRUPT_FF_BIT 7 336 | #define MPU6050_INTERRUPT_MOT_BIT 6 337 | #define MPU6050_INTERRUPT_ZMOT_BIT 5 338 | #define MPU6050_INTERRUPT_FIFO_OFLOW_BIT 4 339 | #define MPU6050_INTERRUPT_I2C_MST_INT_BIT 3 340 | #define MPU6050_INTERRUPT_PLL_RDY_INT_BIT 2 341 | #define MPU6050_INTERRUPT_DMP_INT_BIT 1 342 | #define MPU6050_INTERRUPT_DATA_RDY_BIT 0 343 | 344 | // TODO: figure out what these actually do 345 | // UMPL source code is not very obivous 346 | #define MPU6050_DMPINT_5_BIT 5 347 | #define MPU6050_DMPINT_4_BIT 4 348 | #define MPU6050_DMPINT_3_BIT 3 349 | #define MPU6050_DMPINT_2_BIT 2 350 | #define MPU6050_DMPINT_1_BIT 1 351 | #define MPU6050_DMPINT_0_BIT 0 352 | 353 | #define MPU6050_MOTION_MOT_XNEG_BIT 7 354 | #define MPU6050_MOTION_MOT_XPOS_BIT 6 355 | #define MPU6050_MOTION_MOT_YNEG_BIT 5 356 | #define MPU6050_MOTION_MOT_YPOS_BIT 4 357 | #define MPU6050_MOTION_MOT_ZNEG_BIT 3 358 | #define MPU6050_MOTION_MOT_ZPOS_BIT 2 359 | #define MPU6050_MOTION_MOT_ZRMOT_BIT 0 360 | 361 | #define MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT 7 362 | #define MPU6050_DELAYCTRL_I2C_SLV4_DLY_EN_BIT 4 363 | #define MPU6050_DELAYCTRL_I2C_SLV3_DLY_EN_BIT 3 364 | #define MPU6050_DELAYCTRL_I2C_SLV2_DLY_EN_BIT 2 365 | #define MPU6050_DELAYCTRL_I2C_SLV1_DLY_EN_BIT 1 366 | #define MPU6050_DELAYCTRL_I2C_SLV0_DLY_EN_BIT 0 367 | 368 | #define MPU6050_PATHRESET_GYRO_RESET_BIT 2 369 | #define MPU6050_PATHRESET_ACCEL_RESET_BIT 1 370 | #define MPU6050_PATHRESET_TEMP_RESET_BIT 0 371 | 372 | #define MPU6050_DETECT_ACCEL_ON_DELAY_BIT 5 373 | #define MPU6050_DETECT_ACCEL_ON_DELAY_LENGTH 2 374 | #define MPU6050_DETECT_FF_COUNT_BIT 3 375 | #define MPU6050_DETECT_FF_COUNT_LENGTH 2 376 | #define MPU6050_DETECT_MOT_COUNT_BIT 1 377 | #define MPU6050_DETECT_MOT_COUNT_LENGTH 2 378 | 379 | #define MPU6050_DETECT_DECREMENT_RESET 0x0 380 | #define MPU6050_DETECT_DECREMENT_1 0x1 381 | #define MPU6050_DETECT_DECREMENT_2 0x2 382 | #define MPU6050_DETECT_DECREMENT_4 0x3 383 | 384 | #define MPU6050_USERCTRL_DMP_EN_BIT 7 385 | #define MPU6050_USERCTRL_FIFO_EN_BIT 6 386 | #define MPU6050_USERCTRL_I2C_MST_EN_BIT 5 387 | #define MPU6050_USERCTRL_I2C_IF_DIS_BIT 4 388 | #define MPU6050_USERCTRL_DMP_RESET_BIT 3 389 | #define MPU6050_USERCTRL_FIFO_RESET_BIT 2 390 | #define MPU6050_USERCTRL_I2C_MST_RESET_BIT 1 391 | #define MPU6050_USERCTRL_SIG_COND_RESET_BIT 0 392 | 393 | #define MPU6050_PWR1_DEVICE_RESET_BIT 7 394 | #define MPU6050_PWR1_SLEEP_BIT 6 395 | #define MPU6050_PWR1_CYCLE_BIT 5 396 | #define MPU6050_PWR1_TEMP_DIS_BIT 3 397 | #define MPU6050_PWR1_CLKSEL_BIT 2 398 | #define MPU6050_PWR1_CLKSEL_LENGTH 3 399 | 400 | #define MPU6050_CLOCK_INTERNAL 0x00 401 | #define MPU6050_CLOCK_PLL_XGYRO 0x01 402 | #define MPU6050_CLOCK_PLL_YGYRO 0x02 403 | #define MPU6050_CLOCK_PLL_ZGYRO 0x03 404 | #define MPU6050_CLOCK_PLL_EXT32K 0x04 405 | #define MPU6050_CLOCK_PLL_EXT19M 0x05 406 | #define MPU6050_CLOCK_KEEP_RESET 0x07 407 | 408 | #define MPU6050_PWR2_LP_WAKE_CTRL_BIT 7 409 | #define MPU6050_PWR2_LP_WAKE_CTRL_LENGTH 2 410 | #define MPU6050_PWR2_STBY_XA_BIT 5 411 | #define MPU6050_PWR2_STBY_YA_BIT 4 412 | #define MPU6050_PWR2_STBY_ZA_BIT 3 413 | #define MPU6050_PWR2_STBY_XG_BIT 2 414 | #define MPU6050_PWR2_STBY_YG_BIT 1 415 | #define MPU6050_PWR2_STBY_ZG_BIT 0 416 | 417 | #define MPU6050_WAKE_FREQ_1P25 0x0 418 | #define MPU6050_WAKE_FREQ_2P5 0x1 419 | #define MPU6050_WAKE_FREQ_5 0x2 420 | #define MPU6050_WAKE_FREQ_10 0x3 421 | 422 | #define MPU6050_BANKSEL_PRFTCH_EN_BIT 6 423 | #define MPU6050_BANKSEL_CFG_USER_BANK_BIT 5 424 | #define MPU6050_BANKSEL_MEM_SEL_BIT 4 425 | #define MPU6050_BANKSEL_MEM_SEL_LENGTH 5 426 | 427 | #define MPU6050_WHO_AM_I_BIT 6 428 | #define MPU6050_WHO_AM_I_LENGTH 6 429 | 430 | #define MPU6050_DMP_MEMORY_BANKS 8 431 | #define MPU6050_DMP_MEMORY_BANK_SIZE 256 432 | #define MPU6050_DMP_MEMORY_CHUNK_SIZE 16 433 | 434 | // note: DMP code memory blocks defined at end of header file 435 | 436 | class MPU6050 { 437 | public: 438 | MPU6050(); 439 | MPU6050(uint8_t address); 440 | 441 | void initialize(); 442 | bool testConnection(); 443 | 444 | // AUX_VDDIO register 445 | uint8_t getAuxVDDIOLevel(); 446 | void setAuxVDDIOLevel(uint8_t level); 447 | 448 | // SMPLRT_DIV register 449 | uint8_t getRate(); 450 | void setRate(uint8_t rate); 451 | 452 | // CONFIG register 453 | uint8_t getExternalFrameSync(); 454 | void setExternalFrameSync(uint8_t sync); 455 | uint8_t getDLPFMode(); 456 | void setDLPFMode(uint8_t bandwidth); 457 | 458 | // GYRO_CONFIG register 459 | uint8_t getFullScaleGyroRange(); 460 | void setFullScaleGyroRange(uint8_t range); 461 | 462 | // SELF_TEST registers 463 | uint8_t getAccelXSelfTestFactoryTrim(); 464 | uint8_t getAccelYSelfTestFactoryTrim(); 465 | uint8_t getAccelZSelfTestFactoryTrim(); 466 | 467 | uint8_t getGyroXSelfTestFactoryTrim(); 468 | uint8_t getGyroYSelfTestFactoryTrim(); 469 | uint8_t getGyroZSelfTestFactoryTrim(); 470 | 471 | // ACCEL_CONFIG register 472 | bool getAccelXSelfTest(); 473 | void setAccelXSelfTest(bool enabled); 474 | bool getAccelYSelfTest(); 475 | void setAccelYSelfTest(bool enabled); 476 | bool getAccelZSelfTest(); 477 | void setAccelZSelfTest(bool enabled); 478 | uint8_t getFullScaleAccelRange(); 479 | void setFullScaleAccelRange(uint8_t range); 480 | uint8_t getDHPFMode(); 481 | void setDHPFMode(uint8_t mode); 482 | 483 | // FF_THR register 484 | uint8_t getFreefallDetectionThreshold(); 485 | void setFreefallDetectionThreshold(uint8_t threshold); 486 | 487 | // FF_DUR register 488 | uint8_t getFreefallDetectionDuration(); 489 | void setFreefallDetectionDuration(uint8_t duration); 490 | 491 | // MOT_THR register 492 | uint8_t getMotionDetectionThreshold(); 493 | void setMotionDetectionThreshold(uint8_t threshold); 494 | 495 | // MOT_DUR register 496 | uint8_t getMotionDetectionDuration(); 497 | void setMotionDetectionDuration(uint8_t duration); 498 | 499 | // ZRMOT_THR register 500 | uint8_t getZeroMotionDetectionThreshold(); 501 | void setZeroMotionDetectionThreshold(uint8_t threshold); 502 | 503 | // ZRMOT_DUR register 504 | uint8_t getZeroMotionDetectionDuration(); 505 | void setZeroMotionDetectionDuration(uint8_t duration); 506 | 507 | // FIFO_EN register 508 | bool getTempFIFOEnabled(); 509 | void setTempFIFOEnabled(bool enabled); 510 | bool getXGyroFIFOEnabled(); 511 | void setXGyroFIFOEnabled(bool enabled); 512 | bool getYGyroFIFOEnabled(); 513 | void setYGyroFIFOEnabled(bool enabled); 514 | bool getZGyroFIFOEnabled(); 515 | void setZGyroFIFOEnabled(bool enabled); 516 | bool getAccelFIFOEnabled(); 517 | void setAccelFIFOEnabled(bool enabled); 518 | bool getSlave2FIFOEnabled(); 519 | void setSlave2FIFOEnabled(bool enabled); 520 | bool getSlave1FIFOEnabled(); 521 | void setSlave1FIFOEnabled(bool enabled); 522 | bool getSlave0FIFOEnabled(); 523 | void setSlave0FIFOEnabled(bool enabled); 524 | 525 | // I2C_MST_CTRL register 526 | bool getMultiMasterEnabled(); 527 | void setMultiMasterEnabled(bool enabled); 528 | bool getWaitForExternalSensorEnabled(); 529 | void setWaitForExternalSensorEnabled(bool enabled); 530 | bool getSlave3FIFOEnabled(); 531 | void setSlave3FIFOEnabled(bool enabled); 532 | bool getSlaveReadWriteTransitionEnabled(); 533 | void setSlaveReadWriteTransitionEnabled(bool enabled); 534 | uint8_t getMasterClockSpeed(); 535 | void setMasterClockSpeed(uint8_t speed); 536 | 537 | // I2C_SLV* registers (Slave 0-3) 538 | uint8_t getSlaveAddress(uint8_t num); 539 | void setSlaveAddress(uint8_t num, uint8_t address); 540 | uint8_t getSlaveRegister(uint8_t num); 541 | void setSlaveRegister(uint8_t num, uint8_t reg); 542 | bool getSlaveEnabled(uint8_t num); 543 | void setSlaveEnabled(uint8_t num, bool enabled); 544 | bool getSlaveWordByteSwap(uint8_t num); 545 | void setSlaveWordByteSwap(uint8_t num, bool enabled); 546 | bool getSlaveWriteMode(uint8_t num); 547 | void setSlaveWriteMode(uint8_t num, bool mode); 548 | bool getSlaveWordGroupOffset(uint8_t num); 549 | void setSlaveWordGroupOffset(uint8_t num, bool enabled); 550 | uint8_t getSlaveDataLength(uint8_t num); 551 | void setSlaveDataLength(uint8_t num, uint8_t length); 552 | 553 | // I2C_SLV* registers (Slave 4) 554 | uint8_t getSlave4Address(); 555 | void setSlave4Address(uint8_t address); 556 | uint8_t getSlave4Register(); 557 | void setSlave4Register(uint8_t reg); 558 | void setSlave4OutputByte(uint8_t data); 559 | bool getSlave4Enabled(); 560 | void setSlave4Enabled(bool enabled); 561 | bool getSlave4InterruptEnabled(); 562 | void setSlave4InterruptEnabled(bool enabled); 563 | bool getSlave4WriteMode(); 564 | void setSlave4WriteMode(bool mode); 565 | uint8_t getSlave4MasterDelay(); 566 | void setSlave4MasterDelay(uint8_t delay); 567 | uint8_t getSlate4InputByte(); 568 | 569 | // I2C_MST_STATUS register 570 | bool getPassthroughStatus(); 571 | bool getSlave4IsDone(); 572 | bool getLostArbitration(); 573 | bool getSlave4Nack(); 574 | bool getSlave3Nack(); 575 | bool getSlave2Nack(); 576 | bool getSlave1Nack(); 577 | bool getSlave0Nack(); 578 | 579 | // INT_PIN_CFG register 580 | bool getInterruptMode(); 581 | void setInterruptMode(bool mode); 582 | bool getInterruptDrive(); 583 | void setInterruptDrive(bool drive); 584 | bool getInterruptLatch(); 585 | void setInterruptLatch(bool latch); 586 | bool getInterruptLatchClear(); 587 | void setInterruptLatchClear(bool clear); 588 | bool getFSyncInterruptLevel(); 589 | void setFSyncInterruptLevel(bool level); 590 | bool getFSyncInterruptEnabled(); 591 | void setFSyncInterruptEnabled(bool enabled); 592 | bool getI2CBypassEnabled(); 593 | void setI2CBypassEnabled(bool enabled); 594 | bool getClockOutputEnabled(); 595 | void setClockOutputEnabled(bool enabled); 596 | 597 | // INT_ENABLE register 598 | uint8_t getIntEnabled(); 599 | void setIntEnabled(uint8_t enabled); 600 | bool getIntFreefallEnabled(); 601 | void setIntFreefallEnabled(bool enabled); 602 | bool getIntMotionEnabled(); 603 | void setIntMotionEnabled(bool enabled); 604 | bool getIntZeroMotionEnabled(); 605 | void setIntZeroMotionEnabled(bool enabled); 606 | bool getIntFIFOBufferOverflowEnabled(); 607 | void setIntFIFOBufferOverflowEnabled(bool enabled); 608 | bool getIntI2CMasterEnabled(); 609 | void setIntI2CMasterEnabled(bool enabled); 610 | bool getIntDataReadyEnabled(); 611 | void setIntDataReadyEnabled(bool enabled); 612 | 613 | // INT_STATUS register 614 | uint8_t getIntStatus(); 615 | bool getIntFreefallStatus(); 616 | bool getIntMotionStatus(); 617 | bool getIntZeroMotionStatus(); 618 | bool getIntFIFOBufferOverflowStatus(); 619 | bool getIntI2CMasterStatus(); 620 | bool getIntDataReadyStatus(); 621 | 622 | // ACCEL_*OUT_* registers 623 | 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); 624 | void getMotion6(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz); 625 | void getAcceleration(int16_t* x, int16_t* y, int16_t* z); 626 | int16_t getAccelerationX(); 627 | int16_t getAccelerationY(); 628 | int16_t getAccelerationZ(); 629 | 630 | // TEMP_OUT_* registers 631 | int16_t getTemperature(); 632 | 633 | // GYRO_*OUT_* registers 634 | void getRotation(int16_t* x, int16_t* y, int16_t* z); 635 | int16_t getRotationX(); 636 | int16_t getRotationY(); 637 | int16_t getRotationZ(); 638 | 639 | // EXT_SENS_DATA_* registers 640 | uint8_t getExternalSensorByte(int position); 641 | uint16_t getExternalSensorWord(int position); 642 | uint32_t getExternalSensorDWord(int position); 643 | 644 | // MOT_DETECT_STATUS register 645 | uint8_t getMotionStatus(); 646 | bool getXNegMotionDetected(); 647 | bool getXPosMotionDetected(); 648 | bool getYNegMotionDetected(); 649 | bool getYPosMotionDetected(); 650 | bool getZNegMotionDetected(); 651 | bool getZPosMotionDetected(); 652 | bool getZeroMotionDetected(); 653 | 654 | // I2C_SLV*_DO register 655 | void setSlaveOutputByte(uint8_t num, uint8_t data); 656 | 657 | // I2C_MST_DELAY_CTRL register 658 | bool getExternalShadowDelayEnabled(); 659 | void setExternalShadowDelayEnabled(bool enabled); 660 | bool getSlaveDelayEnabled(uint8_t num); 661 | void setSlaveDelayEnabled(uint8_t num, bool enabled); 662 | 663 | // SIGNAL_PATH_RESET register 664 | void resetGyroscopePath(); 665 | void resetAccelerometerPath(); 666 | void resetTemperaturePath(); 667 | 668 | // MOT_DETECT_CTRL register 669 | uint8_t getAccelerometerPowerOnDelay(); 670 | void setAccelerometerPowerOnDelay(uint8_t delay); 671 | uint8_t getFreefallDetectionCounterDecrement(); 672 | void setFreefallDetectionCounterDecrement(uint8_t decrement); 673 | uint8_t getMotionDetectionCounterDecrement(); 674 | void setMotionDetectionCounterDecrement(uint8_t decrement); 675 | 676 | // USER_CTRL register 677 | bool getFIFOEnabled(); 678 | void setFIFOEnabled(bool enabled); 679 | bool getI2CMasterModeEnabled(); 680 | void setI2CMasterModeEnabled(bool enabled); 681 | void switchSPIEnabled(bool enabled); 682 | void resetFIFO(); 683 | void resetI2CMaster(); 684 | void resetSensors(); 685 | 686 | // PWR_MGMT_1 register 687 | void reset(); 688 | bool getSleepEnabled(); 689 | void setSleepEnabled(bool enabled); 690 | bool getWakeCycleEnabled(); 691 | void setWakeCycleEnabled(bool enabled); 692 | bool getTempSensorEnabled(); 693 | void setTempSensorEnabled(bool enabled); 694 | uint8_t getClockSource(); 695 | void setClockSource(uint8_t source); 696 | 697 | // PWR_MGMT_2 register 698 | uint8_t getWakeFrequency(); 699 | void setWakeFrequency(uint8_t frequency); 700 | bool getStandbyXAccelEnabled(); 701 | void setStandbyXAccelEnabled(bool enabled); 702 | bool getStandbyYAccelEnabled(); 703 | void setStandbyYAccelEnabled(bool enabled); 704 | bool getStandbyZAccelEnabled(); 705 | void setStandbyZAccelEnabled(bool enabled); 706 | bool getStandbyXGyroEnabled(); 707 | void setStandbyXGyroEnabled(bool enabled); 708 | bool getStandbyYGyroEnabled(); 709 | void setStandbyYGyroEnabled(bool enabled); 710 | bool getStandbyZGyroEnabled(); 711 | void setStandbyZGyroEnabled(bool enabled); 712 | 713 | // FIFO_COUNT_* registers 714 | uint16_t getFIFOCount(); 715 | 716 | // FIFO_R_W register 717 | uint8_t getFIFOByte(); 718 | void setFIFOByte(uint8_t data); 719 | void getFIFOBytes(uint8_t *data, uint8_t length); 720 | 721 | // WHO_AM_I register 722 | uint8_t getDeviceID(); 723 | void setDeviceID(uint8_t id); 724 | 725 | // ======== UNDOCUMENTED/DMP REGISTERS/METHODS ======== 726 | 727 | // XG_OFFS_TC register 728 | uint8_t getOTPBankValid(); 729 | void setOTPBankValid(bool enabled); 730 | int8_t getXGyroOffsetTC(); 731 | void setXGyroOffsetTC(int8_t offset); 732 | 733 | // YG_OFFS_TC register 734 | int8_t getYGyroOffsetTC(); 735 | void setYGyroOffsetTC(int8_t offset); 736 | 737 | // ZG_OFFS_TC register 738 | int8_t getZGyroOffsetTC(); 739 | void setZGyroOffsetTC(int8_t offset); 740 | 741 | // X_FINE_GAIN register 742 | int8_t getXFineGain(); 743 | void setXFineGain(int8_t gain); 744 | 745 | // Y_FINE_GAIN register 746 | int8_t getYFineGain(); 747 | void setYFineGain(int8_t gain); 748 | 749 | // Z_FINE_GAIN register 750 | int8_t getZFineGain(); 751 | void setZFineGain(int8_t gain); 752 | 753 | // XA_OFFS_* registers 754 | int16_t getXAccelOffset(); 755 | void setXAccelOffset(int16_t offset); 756 | 757 | // YA_OFFS_* register 758 | int16_t getYAccelOffset(); 759 | void setYAccelOffset(int16_t offset); 760 | 761 | // ZA_OFFS_* register 762 | int16_t getZAccelOffset(); 763 | void setZAccelOffset(int16_t offset); 764 | 765 | // XG_OFFS_USR* registers 766 | int16_t getXGyroOffset(); 767 | void setXGyroOffset(int16_t offset); 768 | 769 | // YG_OFFS_USR* register 770 | int16_t getYGyroOffset(); 771 | void setYGyroOffset(int16_t offset); 772 | 773 | // ZG_OFFS_USR* register 774 | int16_t getZGyroOffset(); 775 | void setZGyroOffset(int16_t offset); 776 | 777 | // INT_ENABLE register (DMP functions) 778 | bool getIntPLLReadyEnabled(); 779 | void setIntPLLReadyEnabled(bool enabled); 780 | bool getIntDMPEnabled(); 781 | void setIntDMPEnabled(bool enabled); 782 | 783 | // DMP_INT_STATUS 784 | bool getDMPInt5Status(); 785 | bool getDMPInt4Status(); 786 | bool getDMPInt3Status(); 787 | bool getDMPInt2Status(); 788 | bool getDMPInt1Status(); 789 | bool getDMPInt0Status(); 790 | 791 | // INT_STATUS register (DMP functions) 792 | bool getIntPLLReadyStatus(); 793 | bool getIntDMPStatus(); 794 | 795 | // USER_CTRL register (DMP functions) 796 | bool getDMPEnabled(); 797 | void setDMPEnabled(bool enabled); 798 | void resetDMP(); 799 | 800 | // BANK_SEL register 801 | void setMemoryBank(uint8_t bank, bool prefetchEnabled=false, bool userBank=false); 802 | 803 | // MEM_START_ADDR register 804 | void setMemoryStartAddress(uint8_t address); 805 | 806 | // MEM_R_W register 807 | uint8_t readMemoryByte(); 808 | void writeMemoryByte(uint8_t data); 809 | void readMemoryBlock(uint8_t *data, uint16_t dataSize, uint8_t bank=0, uint8_t address=0); 810 | bool writeMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank=0, uint8_t address=0, bool verify=true, bool useProgMem=false); 811 | bool writeProgMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank=0, uint8_t address=0, bool verify=true); 812 | 813 | bool writeDMPConfigurationSet(const uint8_t *data, uint16_t dataSize, bool useProgMem=false); 814 | bool writeProgDMPConfigurationSet(const uint8_t *data, uint16_t dataSize); 815 | 816 | // DMP_CFG_1 register 817 | uint8_t getDMPConfig1(); 818 | void setDMPConfig1(uint8_t config); 819 | 820 | // DMP_CFG_2 register 821 | uint8_t getDMPConfig2(); 822 | void setDMPConfig2(uint8_t config); 823 | 824 | // special methods for MotionApps 2.0 implementation 825 | #ifdef MPU6050_INCLUDE_DMP_MOTIONAPPS20 826 | uint8_t *dmpPacketBuffer; 827 | uint16_t dmpPacketSize; 828 | 829 | uint8_t dmpInitialize(); 830 | bool dmpPacketAvailable(); 831 | 832 | uint8_t dmpSetFIFORate(uint8_t fifoRate); 833 | uint8_t dmpGetFIFORate(); 834 | uint8_t dmpGetSampleStepSizeMS(); 835 | uint8_t dmpGetSampleFrequency(); 836 | int32_t dmpDecodeTemperature(int8_t tempReg); 837 | 838 | // Register callbacks after a packet of FIFO data is processed 839 | //uint8_t dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority); 840 | //uint8_t dmpUnregisterFIFORateProcess(inv_obj_func func); 841 | uint8_t dmpRunFIFORateProcesses(); 842 | 843 | // Setup FIFO for various output 844 | uint8_t dmpSendQuaternion(uint_fast16_t accuracy); 845 | uint8_t dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy); 846 | uint8_t dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy); 847 | uint8_t dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy); 848 | uint8_t dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy); 849 | uint8_t dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy); 850 | uint8_t dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy); 851 | uint8_t dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy); 852 | uint8_t dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy); 853 | uint8_t dmpSendPacketNumber(uint_fast16_t accuracy); 854 | uint8_t dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy); 855 | uint8_t dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy); 856 | 857 | // Get Fixed Point data from FIFO 858 | uint8_t dmpGetAccel(int32_t *data, const uint8_t* packet=0); 859 | uint8_t dmpGetAccel(int16_t *data, const uint8_t* packet=0); 860 | uint8_t dmpGetAccel(VectorInt16 *v, const uint8_t* packet=0); 861 | uint8_t dmpGetQuaternion(int32_t *data, const uint8_t* packet=0); 862 | uint8_t dmpGetQuaternion(int16_t *data, const uint8_t* packet=0); 863 | uint8_t dmpGetQuaternion(Quaternion *q, const uint8_t* packet=0); 864 | uint8_t dmpGet6AxisQuaternion(int32_t *data, const uint8_t* packet=0); 865 | uint8_t dmpGet6AxisQuaternion(int16_t *data, const uint8_t* packet=0); 866 | uint8_t dmpGet6AxisQuaternion(Quaternion *q, const uint8_t* packet=0); 867 | uint8_t dmpGetRelativeQuaternion(int32_t *data, const uint8_t* packet=0); 868 | uint8_t dmpGetRelativeQuaternion(int16_t *data, const uint8_t* packet=0); 869 | uint8_t dmpGetRelativeQuaternion(Quaternion *data, const uint8_t* packet=0); 870 | uint8_t dmpGetGyro(int32_t *data, const uint8_t* packet=0); 871 | uint8_t dmpGetGyro(int16_t *data, const uint8_t* packet=0); 872 | uint8_t dmpGetGyro(VectorInt16 *v, const uint8_t* packet=0); 873 | uint8_t dmpSetLinearAccelFilterCoefficient(float coef); 874 | uint8_t dmpGetLinearAccel(int32_t *data, const uint8_t* packet=0); 875 | uint8_t dmpGetLinearAccel(int16_t *data, const uint8_t* packet=0); 876 | uint8_t dmpGetLinearAccel(VectorInt16 *v, const uint8_t* packet=0); 877 | uint8_t dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity); 878 | uint8_t dmpGetLinearAccelInWorld(int32_t *data, const uint8_t* packet=0); 879 | uint8_t dmpGetLinearAccelInWorld(int16_t *data, const uint8_t* packet=0); 880 | uint8_t dmpGetLinearAccelInWorld(VectorInt16 *v, const uint8_t* packet=0); 881 | uint8_t dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q); 882 | uint8_t dmpGetGyroAndAccelSensor(int32_t *data, const uint8_t* packet=0); 883 | uint8_t dmpGetGyroAndAccelSensor(int16_t *data, const uint8_t* packet=0); 884 | uint8_t dmpGetGyroAndAccelSensor(VectorInt16 *g, VectorInt16 *a, const uint8_t* packet=0); 885 | uint8_t dmpGetGyroSensor(int32_t *data, const uint8_t* packet=0); 886 | uint8_t dmpGetGyroSensor(int16_t *data, const uint8_t* packet=0); 887 | uint8_t dmpGetGyroSensor(VectorInt16 *v, const uint8_t* packet=0); 888 | uint8_t dmpGetControlData(int32_t *data, const uint8_t* packet=0); 889 | uint8_t dmpGetTemperature(int32_t *data, const uint8_t* packet=0); 890 | uint8_t dmpGetGravity(int32_t *data, const uint8_t* packet=0); 891 | uint8_t dmpGetGravity(int16_t *data, const uint8_t* packet=0); 892 | uint8_t dmpGetGravity(VectorInt16 *v, const uint8_t* packet=0); 893 | uint8_t dmpGetGravity(VectorFloat *v, Quaternion *q); 894 | uint8_t dmpGetUnquantizedAccel(int32_t *data, const uint8_t* packet=0); 895 | uint8_t dmpGetUnquantizedAccel(int16_t *data, const uint8_t* packet=0); 896 | uint8_t dmpGetUnquantizedAccel(VectorInt16 *v, const uint8_t* packet=0); 897 | uint8_t dmpGetQuantizedAccel(int32_t *data, const uint8_t* packet=0); 898 | uint8_t dmpGetQuantizedAccel(int16_t *data, const uint8_t* packet=0); 899 | uint8_t dmpGetQuantizedAccel(VectorInt16 *v, const uint8_t* packet=0); 900 | uint8_t dmpGetExternalSensorData(int32_t *data, uint16_t size, const uint8_t* packet=0); 901 | uint8_t dmpGetEIS(int32_t *data, const uint8_t* packet=0); 902 | 903 | uint8_t dmpGetEuler(float *data, Quaternion *q); 904 | uint8_t dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity); 905 | 906 | // Get Floating Point data from FIFO 907 | uint8_t dmpGetAccelFloat(float *data, const uint8_t* packet=0); 908 | uint8_t dmpGetQuaternionFloat(float *data, const uint8_t* packet=0); 909 | 910 | uint8_t dmpProcessFIFOPacket(const unsigned char *dmpData); 911 | uint8_t dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed=NULL); 912 | 913 | uint8_t dmpSetFIFOProcessedCallback(void (*func) (void)); 914 | 915 | uint8_t dmpInitFIFOParam(); 916 | uint8_t dmpCloseFIFO(); 917 | uint8_t dmpSetGyroDataSource(uint8_t source); 918 | uint8_t dmpDecodeQuantizedAccel(); 919 | uint32_t dmpGetGyroSumOfSquare(); 920 | uint32_t dmpGetAccelSumOfSquare(); 921 | void dmpOverrideQuaternion(long *q); 922 | uint16_t dmpGetFIFOPacketSize(); 923 | #endif 924 | 925 | // special methods for MotionApps 4.1 implementation 926 | #ifdef MPU6050_INCLUDE_DMP_MOTIONAPPS41 927 | uint8_t *dmpPacketBuffer; 928 | uint16_t dmpPacketSize; 929 | 930 | uint8_t dmpInitialize(); 931 | bool dmpPacketAvailable(); 932 | 933 | uint8_t dmpSetFIFORate(uint8_t fifoRate); 934 | uint8_t dmpGetFIFORate(); 935 | uint8_t dmpGetSampleStepSizeMS(); 936 | uint8_t dmpGetSampleFrequency(); 937 | int32_t dmpDecodeTemperature(int8_t tempReg); 938 | 939 | // Register callbacks after a packet of FIFO data is processed 940 | //uint8_t dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority); 941 | //uint8_t dmpUnregisterFIFORateProcess(inv_obj_func func); 942 | uint8_t dmpRunFIFORateProcesses(); 943 | 944 | // Setup FIFO for various output 945 | uint8_t dmpSendQuaternion(uint_fast16_t accuracy); 946 | uint8_t dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy); 947 | uint8_t dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy); 948 | uint8_t dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy); 949 | uint8_t dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy); 950 | uint8_t dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy); 951 | uint8_t dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy); 952 | uint8_t dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy); 953 | uint8_t dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy); 954 | uint8_t dmpSendPacketNumber(uint_fast16_t accuracy); 955 | uint8_t dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy); 956 | uint8_t dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy); 957 | 958 | // Get Fixed Point data from FIFO 959 | uint8_t dmpGetAccel(int32_t *data, const uint8_t* packet=0); 960 | uint8_t dmpGetAccel(int16_t *data, const uint8_t* packet=0); 961 | uint8_t dmpGetAccel(VectorInt16 *v, const uint8_t* packet=0); 962 | uint8_t dmpGetQuaternion(int32_t *data, const uint8_t* packet=0); 963 | uint8_t dmpGetQuaternion(int16_t *data, const uint8_t* packet=0); 964 | uint8_t dmpGetQuaternion(Quaternion *q, const uint8_t* packet=0); 965 | uint8_t dmpGet6AxisQuaternion(int32_t *data, const uint8_t* packet=0); 966 | uint8_t dmpGet6AxisQuaternion(int16_t *data, const uint8_t* packet=0); 967 | uint8_t dmpGet6AxisQuaternion(Quaternion *q, const uint8_t* packet=0); 968 | uint8_t dmpGetRelativeQuaternion(int32_t *data, const uint8_t* packet=0); 969 | uint8_t dmpGetRelativeQuaternion(int16_t *data, const uint8_t* packet=0); 970 | uint8_t dmpGetRelativeQuaternion(Quaternion *data, const uint8_t* packet=0); 971 | uint8_t dmpGetGyro(int32_t *data, const uint8_t* packet=0); 972 | uint8_t dmpGetGyro(int16_t *data, const uint8_t* packet=0); 973 | uint8_t dmpGetGyro(VectorInt16 *v, const uint8_t* packet=0); 974 | uint8_t dmpGetMag(int16_t *data, const uint8_t* packet=0); 975 | uint8_t dmpSetLinearAccelFilterCoefficient(float coef); 976 | uint8_t dmpGetLinearAccel(int32_t *data, const uint8_t* packet=0); 977 | uint8_t dmpGetLinearAccel(int16_t *data, const uint8_t* packet=0); 978 | uint8_t dmpGetLinearAccel(VectorInt16 *v, const uint8_t* packet=0); 979 | uint8_t dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity); 980 | uint8_t dmpGetLinearAccelInWorld(int32_t *data, const uint8_t* packet=0); 981 | uint8_t dmpGetLinearAccelInWorld(int16_t *data, const uint8_t* packet=0); 982 | uint8_t dmpGetLinearAccelInWorld(VectorInt16 *v, const uint8_t* packet=0); 983 | uint8_t dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q); 984 | uint8_t dmpGetGyroAndAccelSensor(int32_t *data, const uint8_t* packet=0); 985 | uint8_t dmpGetGyroAndAccelSensor(int16_t *data, const uint8_t* packet=0); 986 | uint8_t dmpGetGyroAndAccelSensor(VectorInt16 *g, VectorInt16 *a, const uint8_t* packet=0); 987 | uint8_t dmpGetGyroSensor(int32_t *data, const uint8_t* packet=0); 988 | uint8_t dmpGetGyroSensor(int16_t *data, const uint8_t* packet=0); 989 | uint8_t dmpGetGyroSensor(VectorInt16 *v, const uint8_t* packet=0); 990 | uint8_t dmpGetControlData(int32_t *data, const uint8_t* packet=0); 991 | uint8_t dmpGetTemperature(int32_t *data, const uint8_t* packet=0); 992 | uint8_t dmpGetGravity(int32_t *data, const uint8_t* packet=0); 993 | uint8_t dmpGetGravity(int16_t *data, const uint8_t* packet=0); 994 | uint8_t dmpGetGravity(VectorInt16 *v, const uint8_t* packet=0); 995 | uint8_t dmpGetGravity(VectorFloat *v, Quaternion *q); 996 | uint8_t dmpGetUnquantizedAccel(int32_t *data, const uint8_t* packet=0); 997 | uint8_t dmpGetUnquantizedAccel(int16_t *data, const uint8_t* packet=0); 998 | uint8_t dmpGetUnquantizedAccel(VectorInt16 *v, const uint8_t* packet=0); 999 | uint8_t dmpGetQuantizedAccel(int32_t *data, const uint8_t* packet=0); 1000 | uint8_t dmpGetQuantizedAccel(int16_t *data, const uint8_t* packet=0); 1001 | uint8_t dmpGetQuantizedAccel(VectorInt16 *v, const uint8_t* packet=0); 1002 | uint8_t dmpGetExternalSensorData(int32_t *data, uint16_t size, const uint8_t* packet=0); 1003 | uint8_t dmpGetEIS(int32_t *data, const uint8_t* packet=0); 1004 | 1005 | uint8_t dmpGetEuler(float *data, Quaternion *q); 1006 | uint8_t dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity); 1007 | 1008 | // Get Floating Point data from FIFO 1009 | uint8_t dmpGetAccelFloat(float *data, const uint8_t* packet=0); 1010 | uint8_t dmpGetQuaternionFloat(float *data, const uint8_t* packet=0); 1011 | 1012 | uint8_t dmpProcessFIFOPacket(const unsigned char *dmpData); 1013 | uint8_t dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed=NULL); 1014 | 1015 | uint8_t dmpSetFIFOProcessedCallback(void (*func) (void)); 1016 | 1017 | uint8_t dmpInitFIFOParam(); 1018 | uint8_t dmpCloseFIFO(); 1019 | uint8_t dmpSetGyroDataSource(uint8_t source); 1020 | uint8_t dmpDecodeQuantizedAccel(); 1021 | uint32_t dmpGetGyroSumOfSquare(); 1022 | uint32_t dmpGetAccelSumOfSquare(); 1023 | void dmpOverrideQuaternion(long *q); 1024 | uint16_t dmpGetFIFOPacketSize(); 1025 | #endif 1026 | 1027 | private: 1028 | uint8_t devAddr; 1029 | uint8_t buffer[14]; 1030 | }; 1031 | 1032 | #endif /* _MPU6050_H_ */ 1033 | -------------------------------------------------------------------------------- /lib/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(xgOffsetTC); 366 | DEBUG_PRINT(F("Y gyro offset = ")); 367 | DEBUG_PRINTLN(ygOffsetTC); 368 | DEBUG_PRINT(F("Z gyro offset = ")); 369 | DEBUG_PRINTLN(zgOffsetTC); 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] = (((uint32_t)packet[28] << 24) | ((uint32_t)packet[29] << 16) | ((uint32_t)packet[30] << 8) | packet[31]); 581 | data[1] = (((uint32_t)packet[32] << 24) | ((uint32_t)packet[33] << 16) | ((uint32_t)packet[34] << 8) | packet[35]); 582 | data[2] = (((uint32_t)packet[36] << 24) | ((uint32_t)packet[37] << 16) | ((uint32_t)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] = (((uint32_t)packet[0] << 24) | ((uint32_t)packet[1] << 16) | ((uint32_t)packet[2] << 8) | packet[3]); 605 | data[1] = (((uint32_t)packet[4] << 24) | ((uint32_t)packet[5] << 16) | ((uint32_t)packet[6] << 8) | packet[7]); 606 | data[2] = (((uint32_t)packet[8] << 24) | ((uint32_t)packet[9] << 16) | ((uint32_t)packet[10] << 8) | packet[11]); 607 | data[3] = (((uint32_t)packet[12] << 24) | ((uint32_t)packet[13] << 16) | ((uint32_t)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] = (((uint32_t)packet[16] << 24) | ((uint32_t)packet[17] << 16) | ((uint32_t)packet[18] << 8) | packet[19]); 638 | data[1] = (((uint32_t)packet[20] << 24) | ((uint32_t)packet[21] << 16) | ((uint32_t)packet[22] << 8) | packet[23]); 639 | data[2] = (((uint32_t)packet[24] << 24) | ((uint32_t)packet[25] << 16) | ((uint32_t)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::dmpGetGyro(VectorInt16 *v, const uint8_t* packet) { 651 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) 652 | if (packet == 0) packet = dmpPacketBuffer; 653 | v -> x = (packet[16] << 8) | packet[17]; 654 | v -> y = (packet[20] << 8) | packet[21]; 655 | v -> z = (packet[24] << 8) | packet[25]; 656 | return 0; 657 | } 658 | // uint8_t MPU6050::dmpSetLinearAccelFilterCoefficient(float coef); 659 | // uint8_t MPU6050::dmpGetLinearAccel(long *data, const uint8_t* packet); 660 | uint8_t MPU6050::dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity) { 661 | // get rid of the gravity component (+1g = +8192 in standard DMP FIFO packet, sensitivity is 2g) 662 | v -> x = vRaw -> x - gravity -> x*8192; 663 | v -> y = vRaw -> y - gravity -> y*8192; 664 | v -> z = vRaw -> z - gravity -> z*8192; 665 | return 0; 666 | } 667 | // uint8_t MPU6050::dmpGetLinearAccelInWorld(long *data, const uint8_t* packet); 668 | uint8_t MPU6050::dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q) { 669 | // rotate measured 3D acceleration vector into original state 670 | // frame of reference based on orientation quaternion 671 | memcpy(v, vReal, sizeof(VectorInt16)); 672 | v -> rotate(q); 673 | return 0; 674 | } 675 | // uint8_t MPU6050::dmpGetGyroAndAccelSensor(long *data, const uint8_t* packet); 676 | // uint8_t MPU6050::dmpGetGyroSensor(long *data, const uint8_t* packet); 677 | // uint8_t MPU6050::dmpGetControlData(long *data, const uint8_t* packet); 678 | // uint8_t MPU6050::dmpGetTemperature(long *data, const uint8_t* packet); 679 | // uint8_t MPU6050::dmpGetGravity(long *data, const uint8_t* packet); 680 | uint8_t MPU6050::dmpGetGravity(VectorFloat *v, Quaternion *q) { 681 | v -> x = 2 * (q -> x*q -> z - q -> w*q -> y); 682 | v -> y = 2 * (q -> w*q -> x + q -> y*q -> z); 683 | v -> z = q -> w*q -> w - q -> x*q -> x - q -> y*q -> y + q -> z*q -> z; 684 | return 0; 685 | } 686 | // uint8_t MPU6050::dmpGetUnquantizedAccel(long *data, const uint8_t* packet); 687 | // uint8_t MPU6050::dmpGetQuantizedAccel(long *data, const uint8_t* packet); 688 | // uint8_t MPU6050::dmpGetExternalSensorData(long *data, int size, const uint8_t* packet); 689 | // uint8_t MPU6050::dmpGetEIS(long *data, const uint8_t* packet); 690 | 691 | uint8_t MPU6050::dmpGetEuler(float *data, Quaternion *q) { 692 | 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 693 | data[1] = -asin(2*q -> x*q -> z + 2*q -> w*q -> y); // theta 694 | 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 695 | return 0; 696 | } 697 | uint8_t MPU6050::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) { 698 | // yaw: (about Z axis) 699 | data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1); 700 | // pitch: (nose up/down, about Y axis) 701 | data[1] = atan(gravity -> x / sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z)); 702 | // roll: (tilt left/right, about X axis) 703 | data[2] = atan(gravity -> y / sqrt(gravity -> x*gravity -> x + gravity -> z*gravity -> z)); 704 | return 0; 705 | } 706 | 707 | // uint8_t MPU6050::dmpGetAccelFloat(float *data, const uint8_t* packet); 708 | // uint8_t MPU6050::dmpGetQuaternionFloat(float *data, const uint8_t* packet); 709 | 710 | uint8_t MPU6050::dmpProcessFIFOPacket(const unsigned char *dmpData) { 711 | /*for (uint8_t k = 0; k < dmpPacketSize; k++) { 712 | if (dmpData[k] < 0x10) Serial.print("0"); 713 | Serial.print(dmpData[k], HEX); 714 | Serial.print(" "); 715 | } 716 | Serial.print("\n");*/ 717 | //Serial.println((uint16_t)dmpPacketBuffer); 718 | return 0; 719 | } 720 | uint8_t MPU6050::dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed) { 721 | uint8_t status; 722 | uint8_t buf[dmpPacketSize]; 723 | for (uint8_t i = 0; i < numPackets; i++) { 724 | // read packet from FIFO 725 | getFIFOBytes(buf, dmpPacketSize); 726 | 727 | // process packet 728 | if ((status = dmpProcessFIFOPacket(buf)) > 0) return status; 729 | 730 | // increment external process count variable, if supplied 731 | if (processed != 0) *processed++; 732 | } 733 | return 0; 734 | } 735 | 736 | // uint8_t MPU6050::dmpSetFIFOProcessedCallback(void (*func) (void)); 737 | 738 | // uint8_t MPU6050::dmpInitFIFOParam(); 739 | // uint8_t MPU6050::dmpCloseFIFO(); 740 | // uint8_t MPU6050::dmpSetGyroDataSource(uint_fast8_t source); 741 | // uint8_t MPU6050::dmpDecodeQuantizedAccel(); 742 | // uint32_t MPU6050::dmpGetGyroSumOfSquare(); 743 | // uint32_t MPU6050::dmpGetAccelSumOfSquare(); 744 | // void MPU6050::dmpOverrideQuaternion(long *q); 745 | uint16_t MPU6050::dmpGetFIFOPacketSize() { 746 | return dmpPacketSize; 747 | } 748 | 749 | #endif /* _MPU6050_6AXIS_MOTIONAPPS20_H_ */ 750 | -------------------------------------------------------------------------------- /lib/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 unsigned char 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 unsigned char 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 unsigned char 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 | setXGyroOffsetTC(xgOffset); 461 | setYGyroOffsetTC(ygOffset); 462 | setZGyroOffsetTC(zgOffset); 463 | 464 | //DEBUG_PRINTLN(F("Setting X/Y/Z gyro user offsets to zero...")); 465 | //setXGyroOffset(0); 466 | //setYGyroOffset(0); 467 | //setZGyroOffset(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] = (((uint32_t)packet[34] << 24) | ((uint32_t)packet[35] << 16) | ((uint32_t)packet[36] << 8) | packet[37]); 684 | data[1] = (((uint32_t)packet[38] << 24) | ((uint32_t)packet[39] << 16) | ((uint32_t)packet[40] << 8) | packet[41]); 685 | data[2] = (((uint32_t)packet[42] << 24) | ((uint32_t)packet[43] << 16) | ((uint32_t)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] = (((uint32_t)packet[0] << 24) | ((uint32_t)packet[1] << 16) | ((uint32_t)packet[2] << 8) | packet[3]); 708 | data[1] = (((uint32_t)packet[4] << 24) | ((uint32_t)packet[5] << 16) | ((uint32_t)packet[6] << 8) | packet[7]); 709 | data[2] = (((uint32_t)packet[8] << 24) | ((uint32_t)packet[9] << 16) | ((uint32_t)packet[10] << 8) | packet[11]); 710 | data[3] = (((uint32_t)packet[12] << 24) | ((uint32_t)packet[13] << 16) | ((uint32_t)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] = (((uint32_t)packet[16] << 24) | ((uint32_t)packet[17] << 16) | ((uint32_t)packet[18] << 8) | packet[19]); 741 | data[1] = (((uint32_t)packet[20] << 24) | ((uint32_t)packet[21] << 16) | ((uint32_t)packet[22] << 8) | packet[23]); 742 | data[2] = (((uint32_t)packet[24] << 24) | ((uint32_t)packet[25] << 16) | ((uint32_t)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 | -------------------------------------------------------------------------------- /lib/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_ */ -------------------------------------------------------------------------------- /lib/MPU6050/library.json: -------------------------------------------------------------------------------- 1 | { 2 | "name": "I2Cdevlib-MPU6050", 3 | "keywords": "gyroscope, accelerometer, sensor, i2cdevlib, i2c", 4 | "description": "The MPU6050 combines a 3-axis gyroscope and a 3-axis accelerometer on the same silicon die together with an onboard Digital Motion Processor(DMP) which processes complex 6-axis MotionFusion algorithms", 5 | "include": "Arduino/MPU6050", 6 | "repository": 7 | { 8 | "type": "git", 9 | "url": "https://github.com/jrowberg/i2cdevlib.git" 10 | }, 11 | "dependencies": 12 | { 13 | "name": "I2Cdevlib-Core", 14 | "frameworks": "arduino" 15 | }, 16 | "frameworks": "arduino", 17 | "platforms": "atmelavr" 18 | } 19 | -------------------------------------------------------------------------------- /lib/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 | 24 | myOutput = Output; 25 | myInput = Input; 26 | mySetpoint = Setpoint; 27 | inAuto = false; 28 | 29 | PID::SetOutputLimits(0, 255); //default output limit corresponds to 30 | //the arduino pwm limits 31 | 32 | SampleTime = 50; //default Controller Sample Time is 0.1 seconds 33 | 34 | PID::SetControllerDirection(ControllerDirection); 35 | PID::SetTunings(Kp, Ki, Kd); 36 | 37 | lastTime = millis()-SampleTime; 38 | } 39 | 40 | 41 | /* Compute() ********************************************************************** 42 | * This, as they say, is where the magic happens. this function should be called 43 | * every time "void loop()" executes. the function will decide for itself whether a new 44 | * pid Output needs to be computed. returns true when the output is computed, 45 | * false when nothing has been done. 46 | **********************************************************************************/ 47 | bool PID::Compute() 48 | { 49 | if(!inAuto) return false; 50 | unsigned long now = millis(); 51 | unsigned long timeChange = (now - lastTime); 52 | if(timeChange>=SampleTime) 53 | { 54 | /*Compute all the working error variables*/ 55 | double input = *myInput; 56 | double error = *mySetpoint - input; 57 | ITerm+= (ki * error); 58 | //ITerm = constrain(ITerm, outMin, outMax); 59 | if(ITerm > outMax) ITerm= outMax; 60 | else if(ITerm < outMin) ITerm= outMin; 61 | double dInput = (input - lastInput); 62 | 63 | /*Compute PID Output*/ 64 | double output = kp * error + ITerm - kd * dInput; 65 | if(output > outMax) output = outMax; 66 | else if(output < outMin) output = outMin; 67 | 68 | //output = constrain(output, outMin, outMax); 69 | 70 | *myOutput = output; 71 | 72 | /*Remember some variables for next time*/ 73 | lastT2input = lastInput; 74 | lastInput = input; 75 | 76 | lastTime = now; 77 | return true; 78 | } 79 | else return false; 80 | } 81 | 82 | 83 | /* SetTunings(...)************************************************************* 84 | * This function allows the controller's dynamic performance to be adjusted. 85 | * it's called automatically from the constructor, but tunings can also 86 | * be adjusted on the fly during normal operation 87 | ******************************************************************************/ 88 | void PID::SetTunings(double Kp, double Ki, double Kd) 89 | { 90 | if (Kp<0 || Ki<0 || Kd<0) return; 91 | 92 | dispKp = Kp; dispKi = Ki; dispKd = Kd; 93 | 94 | double SampleTimeInSec = ((double)SampleTime)/1000; 95 | kp = Kp; 96 | ki = Ki * SampleTimeInSec; 97 | kd = Kd / SampleTimeInSec; 98 | 99 | ITerm = 0; 100 | if(controllerDirection ==REVERSE) 101 | { 102 | kp = (0 - kp); 103 | ki = (0 - ki); 104 | kd = (0 - kd); 105 | } 106 | } 107 | 108 | /* SetSampleTime(...) ********************************************************* 109 | * sets the period, in Milliseconds, at which the calculation is performed 110 | ******************************************************************************/ 111 | void PID::SetSampleTime(int NewSampleTime) 112 | { 113 | if (NewSampleTime > 0) 114 | { 115 | double ratio = (double)NewSampleTime 116 | / (double)SampleTime; 117 | ki *= ratio; 118 | kd /= ratio; 119 | SampleTime = (unsigned long)NewSampleTime; 120 | } 121 | } 122 | 123 | /* SetOutputLimits(...)**************************************************** 124 | * This function will be used far more often than SetInputLimits. while 125 | * the input to the controller will generally be in the 0-1023 range (which is 126 | * the default already,) the output will be a little different. maybe they'll 127 | * be doing a time window and will need 0-8000 or something. or maybe they'll 128 | * want to clamp it from 0-125. who knows. at any rate, that can all be done 129 | * here. 130 | **************************************************************************/ 131 | void PID::SetOutputLimits(double Min, double Max) 132 | { 133 | if(Min >= Max) return; 134 | outMin = Min; 135 | outMax = Max; 136 | 137 | if(inAuto) 138 | { 139 | if(*myOutput > outMax) *myOutput = outMax; 140 | else if(*myOutput < outMin) *myOutput = outMin; 141 | 142 | if(ITerm > outMax) ITerm= outMax; 143 | else if(ITerm < outMin) ITerm= outMin; 144 | } 145 | } 146 | 147 | /* SetMode(...)**************************************************************** 148 | * Allows the controller Mode to be set to manual (0) or Automatic (non-zero) 149 | * when the transition from manual to auto occurs, the controller is 150 | * automatically initialized 151 | ******************************************************************************/ 152 | void PID::SetMode(int Mode) 153 | { 154 | bool newAuto = (Mode == AUTOMATIC); 155 | if(newAuto == !inAuto) 156 | { /*we just went from manual to auto*/ 157 | PID::Initialize(); 158 | } 159 | inAuto = newAuto; 160 | } 161 | 162 | /* Initialize()**************************************************************** 163 | * does all the things that need to happen to ensure a bumpless transfer 164 | * from manual to automatic mode. 165 | ******************************************************************************/ 166 | void PID::Initialize() 167 | { 168 | ITerm = *myOutput; 169 | lastInput = *myInput; 170 | if(ITerm > outMax) ITerm = outMax; 171 | else if(ITerm < outMin) ITerm = outMin; 172 | } 173 | 174 | /* SetControllerDirection(...)************************************************* 175 | * The PID will either be connected to a DIRECT acting process (+Output leads 176 | * to +Input) or a REVERSE acting process(+Output leads to -Input.) we need to 177 | * know which one, because otherwise we may increase the output when we should 178 | * be decreasing. This is called from the constructor. 179 | ******************************************************************************/ 180 | void PID::SetControllerDirection(int Direction) 181 | { 182 | if(inAuto && Direction !=controllerDirection) 183 | { 184 | kp = (0 - kp); 185 | ki = (0 - ki); 186 | kd = (0 - kd); 187 | } 188 | controllerDirection = Direction; 189 | } 190 | 191 | /* Status Funcions************************************************************* 192 | * Just because you set the Kp=-1 doesn't mean it actually happened. these 193 | * functions query the internal state of the PID. they're here for display 194 | * purposes. this are the functions the PID Front-end uses for example 195 | ******************************************************************************/ 196 | double PID::GetKp(){ return dispKp; } 197 | double PID::GetKi(){ return dispKi;} 198 | double PID::GetKd(){ return dispKd;} 199 | int PID::GetMode(){ return inAuto ? AUTOMATIC : MANUAL;} 200 | int PID::GetDirection(){ return controllerDirection;} 201 | 202 | -------------------------------------------------------------------------------- /lib/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 | bool 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, lastT2input; 74 | 75 | unsigned long SampleTime; 76 | double outMin, outMax; 77 | bool inAuto; 78 | }; 79 | #endif 80 | 81 | -------------------------------------------------------------------------------- /lib/PID_v1/keywords.txt: -------------------------------------------------------------------------------- 1 | ####################################### 2 | # Syntax Coloring Map For PID Library 3 | ####################################### 4 | 5 | ####################################### 6 | # Datatypes (KEYWORD1) 7 | ####################################### 8 | 9 | PID KEYWORD1 10 | 11 | ####################################### 12 | # Methods and Functions (KEYWORD2) 13 | ####################################### 14 | 15 | SetMode KEYWORD2 16 | Compute KEYWORD2 17 | SetOutputLimits KEYWORD2 18 | SetTunings KEYWORD2 19 | SetControllerDirection KEYWORD2 20 | SetSampleTime KEYWORD2 21 | GetKp KEYWORD2 22 | GetKi KEYWORD2 23 | GetKd KEYWORD2 24 | GetMode KEYWORD2 25 | GetDirection KEYWORD2 26 | 27 | ####################################### 28 | # Constants (LITERAL1) 29 | ####################################### 30 | 31 | AUTOMATIC LITERAL1 32 | MANUAL LITERAL1 33 | DIRECT LITERAL1 34 | REVERSE LITERAL1 -------------------------------------------------------------------------------- /src/basic_balance_motor_speed.cpp: -------------------------------------------------------------------------------- 1 | #define DEBUG 1 2 | 3 | #include 4 | #include 5 | #include "printf.h" 6 | #include "basic_balance_motor_speed.h" 7 | #include "utils.h" 8 | 9 | //Basic speed calculator based on the angle measured 10 | //if the angle is positive the speed is +max_speed 11 | //if the angle is negative the speed is -max_speed 12 | void get_basic_balance_motor_speed(int16_t * motor_accel, float angle, float angle_old, int16_t m1, int16_t m2) { 13 | int16_t speed = 15 * ((angle < 0) ? -1 : 1); 14 | 15 | #if DEBUG 16 | runEvery(1000) printsf(__func__, "basic accel value: %d\n", speed); 17 | #endif 18 | 19 | motor_accel[0] = speed; 20 | motor_accel[1] = speed; 21 | } 22 | -------------------------------------------------------------------------------- /src/basic_balance_motor_speed.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | extern void get_basic_balance_motor_speed(int16_t *, float, float, int16_t, int16_t); 4 | -------------------------------------------------------------------------------- /src/constants.h: -------------------------------------------------------------------------------- 1 | #ifndef CONSTANTS_H 2 | #define CONSTANTS_H 3 | 4 | double KP=3.25; 5 | double KI=0; 6 | double KD=0.55; 7 | 8 | #define MAX_ACCEL 50 9 | 10 | #define P1_X 500 11 | #define P1_Y 2 12 | 13 | #define P2_X 0 14 | #define P2_Y 30 15 | 16 | #endif 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /src/main.ino: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | // Arduino libraries 4 | #include 5 | #include 6 | #include 7 | 8 | // Local includes 9 | #include "printf.h" 10 | #include "pot_motor_speed.h" 11 | #include "pid_motor_speed.h" 12 | #include "basic_balance_motor_speed.h" 13 | #include "utils.h" 14 | 15 | // ================================================================ 16 | // === CONSTANTS === 17 | // ================================================================ 18 | #define DEBUG 0 19 | 20 | #define MOTOR_1_STEP 6 21 | #define MOTOR_1_DIR 8 22 | 23 | #define MOTOR_2_STEP 12 24 | #define MOTOR_2_DIR 13 25 | 26 | #define LED_PIN 3 27 | 28 | // ================================================================ 29 | // === Globals === 30 | // ================================================================ 31 | int16_t motor_1_speed; 32 | int16_t motor_2_speed; 33 | int16_t speed_period_m1; 34 | int16_t speed_period_m2; 35 | uint8_t running_mode; 36 | 37 | // MPU 38 | MPU6050 mpu; 39 | 40 | // ================================================================ 41 | // === MPU FUNCTIONS === 42 | // ================================================================ 43 | float dmpGetPhi() { 44 | uint8_t fifoBuffer[64]; // FIFO storage buffer 45 | VectorFloat gravity; 46 | Quaternion q; 47 | float ypr[3]; 48 | 49 | mpu.getFIFOBytes(fifoBuffer, 16); // We only read the quaternion 50 | mpu.dmpGetQuaternion(&q, fifoBuffer); 51 | mpu.resetFIFO(); // We always reset FIFO 52 | float result = ( asin(-2*(q.x * q.z - q.w * q.y)) * 180/M_PI); 53 | 54 | return result; 55 | 56 | } 57 | 58 | // ================================================================ 59 | // === MPU SETUP === 60 | // ================================================================ 61 | void setup_mpu(){ 62 | uint16_t packetSize; // expected DMP packet size (default is 42 bytes) 63 | 64 | // run scl at 400khz 65 | TWSR = 0; 66 | TWBR = ((16000000L/400000L)-16)/2; 67 | TWCR = 1< 0 ? HIGH : LOW); 124 | digitalWrite(MOTOR_2_DIR, motor_2_speed >= 0 ? HIGH : LOW); 125 | 126 | if (counter_m1 >= speed_period_m1) { 127 | counter_m1 = 0; 128 | step(MOTOR_1_STEP); 129 | } 130 | 131 | if (counter_m2 >= speed_period_m2) { 132 | counter_m2 = 0; 133 | step(MOTOR_2_STEP); 134 | } 135 | } 136 | // Modules functions 137 | static void (*function[24])(int16_t *, float, float, int16_t, int16_t); 138 | static uint16_t module_counter; 139 | 140 | static void register_module(void (*func)(int16_t *, float, float, int16_t, int16_t)) { 141 | function[module_counter++] = func; 142 | } 143 | //----------------------------------------- 144 | // Timer Setup 145 | // ---------------------------------------- 146 | // timer setup, frequency in milliseconds as argument - we are using a divider of 8 147 | // resulting in a 2MHz on a 16MHz cpu 148 | // 40 - 25Khz 149 | void setup_timer(uint16_t freq) { 150 | TCCR1B &= ~(1<= 18) { 210 | float angle_adjusted_old = angle_adjusted; 211 | angle_adjusted = dmpGetPhi(); 212 | // if we are in an recoverable position 213 | if (angle_adjusted > -35 && angle_adjusted < 35) { 214 | int16_t motor_accel[2]; 215 | // call the running module 216 | (*function[running_mode])(motor_accel, angle_adjusted, angle_adjusted_old, motor_1_speed, motor_2_speed); 217 | 218 | // we integrate the acceleration 219 | motor_1_speed += motor_accel[0]; 220 | motor_2_speed -= motor_accel[1]; 221 | 222 | //apply steering 223 | /* 224 | bool should = millis() - stimer > 5000 && millis() - stimer < 5200; 225 | 226 | if (should) { 227 | motor_1_speed += 15; 228 | motor_2_speed -= 15; 229 | } else { 230 | motor_2_speed = -motor_1_speed; 231 | } 232 | */ 233 | 234 | //constrain motor speed 235 | motor_1_speed = constrain(motor_1_speed, -500, 500); 236 | motor_2_speed = constrain(motor_2_speed, -500, 500); 237 | 238 | // calculate motor period by the function of f(x)=-0.017*x+10.5 239 | speed_period_m1 = -0.054*abs(motor_1_speed) + 30; 240 | speed_period_m2 = -0.054*abs(motor_2_speed) + 30; 241 | } else if (angle_adjusted == angle_adjusted) { 242 | // if it is an angle we can't recover we gg and stop the motors 243 | motor_1_speed = motor_2_speed = 0; 244 | speed_period_m1 = speed_period_m2 = 0; 245 | } 246 | } 247 | 248 | #if DEBUG 249 | runEvery(1000) printsf(__func__, "M1: %d M2: %d angle: %d\n", motor_1_speed, motor_2_speed, (int)angle_adjusted); 250 | #endif 251 | 252 | } 253 | -------------------------------------------------------------------------------- /src/pid_chain_motor_speed.cpp: -------------------------------------------------------------------------------- 1 | #define DEBUG 0 2 | 3 | #include 4 | #include 5 | #include "PID_v1.h" 6 | #include "printf.h" 7 | #include "utils.h" 8 | 9 | double pid_chain_setpoint_angle, pid_chain_input_angle, pid_chain_output_angle; 10 | double pid_chain_setpoint_speed, pid_chain_input_speed, pid_chain_output_accel; 11 | 12 | // pid to obtain the desired angle given the current speed and the target speed 13 | PID pid_chain_controller_angle(&pid_chain_input_speed, &pid_chain_output_angle, &pid_chain_setpoint_speed, 0.1, 0.1, 0.1, DIRECT); 14 | // pid to obtain the desired speed given the current angle and the desired angle 15 | PID pid_chain_controller_speed(&pid_chain_input_angle, &pid_chain_output_accel, &pid_chain_setpoint_angle, 2, 0.00, 0.2, DIRECT); 16 | 17 | void setup_pid_chain_chain() { 18 | pid_chain_controller_angle.SetMode(AUTOMATIC); 19 | pid_chain_controller_angle.SetOutputLimits(-35, 35); 20 | 21 | pid_chain_controller_speed.SetMode(AUTOMATIC); 22 | pid_chain_controller_speed.SetOutputLimits(-40, 40); 23 | } 24 | 25 | // * Get the current angle from the MPU 26 | // * Estimate the current speed 27 | // * Feed the current speed to the angle PID to obtain the desired angle to get to the input speed 28 | // * Feed the current angle to the speed PID to obtain the desired acceleration to get to the desired angle 29 | // * Adjust motor speeds accordingly 30 | void get_pid_chain_motor_speed(int16_t * motor_accel, float angle, float angle_old, int16_t m1, int16_t m2) { 31 | // for now our target speed **ALWAYS** is zero 32 | pid_chain_setpoint_speed = 0; 33 | // we need to predict our current speed for the inputs we have 34 | int angular_velocity = (angle - angle_old) * 90; 35 | pid_chain_input_speed = (m1 - m2) / 2 - (pid_chain_input_speed-angular_velocity); 36 | 37 | pid_chain_controller_angle.Compute(); 38 | 39 | // PID given the target angle and the current angle 40 | // outputs the target speed 41 | pid_chain_setpoint_angle = pid_chain_output_angle; 42 | pid_chain_input_angle = angle; 43 | pid_chain_controller_speed.Compute(); 44 | 45 | #if DEBUG 46 | runEvery(1000) { 47 | printsf(__func__, "Speed PID: setpoint: %f input: %f output (acceleration): %f", pid_chain_setpoint_angle, pid_chain_input_angle, pid_chain_output_accel); 48 | } 49 | #endif 50 | 51 | motor_accel[0] = -(int16_t)pid_chain_output_accel; 52 | motor_accel[1] = -(int16_t)pid_chain_output_accel; 53 | } 54 | -------------------------------------------------------------------------------- /src/pid_chain_motor_speed.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | extern void setup_pid_chain(); 4 | extern void get_pid_chain_motor_speed(int16_t *, float, float, int16_t, int16_t); 5 | -------------------------------------------------------------------------------- /src/pid_motor_speed.cpp: -------------------------------------------------------------------------------- 1 | #define DEBUG 0 2 | 3 | #include 4 | #include 5 | #include "PID_v1.h" 6 | #include "printf.h" 7 | #include "utils.h" 8 | #include "constants.h" 9 | 10 | double pid_output_accel, pid_setpoint_angle, pid_input_angle; 11 | 12 | // pid to obtain the desired speed given the current angle and the desired angle 13 | PID pid_controller_speed(&pid_input_angle, &pid_output_accel, &pid_setpoint_angle, KP, KI, KD, DIRECT); 14 | 15 | void setup_pid() { 16 | pid_controller_speed.SetMode(AUTOMATIC); 17 | pid_controller_speed.SetOutputLimits(-MAX_ACCEL, MAX_ACCEL); 18 | pid_controller_speed.SetSampleTime(10); // 10ms 19 | pid_controller_speed.SetTunings(KP, KI, KD); 20 | pid_setpoint_angle = -2; // for now we'll ignore the first pid computation 21 | } 22 | 23 | // * Get the current angle from the MPU 24 | // * Feed the current angle to the speed PID to obtain the desired acceleration to get to the desired angle 25 | // * Adjust motor speeds accordingly 26 | void get_pid_motor_speed(int16_t * motor_accel, float angle, float angle_old, int16_t m1, int16_t m2) { 27 | 28 | // manual tunning via serial 29 | if (Serial.available() > 0) { 30 | int incomingByte = Serial.read(); 31 | 32 | Serial.println(incomingByte); 33 | //1 - KP += 0.1 34 | //2 - KP -= 0.1 35 | //3 - KI += 0.1 36 | //4 - KI -= 0.1 37 | 38 | if ((incomingByte - 48) == 1) KP += 0.01; 39 | if ((incomingByte - 48) == 2) KP -= 0.01; 40 | if ((incomingByte - 48) == 4) KI += 0.01; 41 | if ((incomingByte - 48) == 5) KI -= 0.01; 42 | if ((incomingByte - 48) == 7) KD += 0.01; 43 | if ((incomingByte - 48) == 8) KD -= 0.01; 44 | 45 | 46 | pid_controller_speed.SetTunings(KP, KI, KD); 47 | Serial.print("current values = "); 48 | Serial.print(KP); 49 | Serial.print(" "); 50 | 51 | Serial.print(KI); 52 | Serial.print(" "); 53 | Serial.println(KD); 54 | } 55 | 56 | pid_input_angle = angle; 57 | pid_controller_speed.Compute(); 58 | 59 | motor_accel[0] = (int16_t)pid_output_accel; 60 | motor_accel[1] = (int16_t)pid_output_accel; 61 | 62 | #if DEBUG 63 | runEvery(500) { 64 | printsf(__func__, "Speed PID: setpoint: %d input: %d output (acceleration): %d\n", (int16_t)pid_setpoint_angle, (int16_t)pid_input_angle, (int16_t)pid_output_accel); 65 | } 66 | #endif 67 | 68 | } 69 | -------------------------------------------------------------------------------- /src/pid_motor_speed.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | extern void setup_pid(); 4 | extern void get_pid_motor_speed(int16_t *, float, float, int16_t, int16_t); 5 | -------------------------------------------------------------------------------- /src/pot_motor_speed.cpp: -------------------------------------------------------------------------------- 1 | #define DEBUG 1 2 | 3 | #include 4 | #include 5 | #include 6 | #include "printf.h" 7 | #include "utils.h" 8 | 9 | // ================================================================ 10 | // === CONSTANTS === 11 | // ================================================================ 12 | #define POTENT 3 13 | 14 | 15 | void get_pot_motor_speed(int16_t * motor_accel, float angle, float angle_old, int16_t m1, int16_t m2) { 16 | int16_t val = map(analogRead(POTENT), 0, 1020, -15, 15); 17 | 18 | #if DEBUG 19 | runEvery(1000) { 20 | printsf(__func__, "Potentiometer accel value: %d\n", val); 21 | //Serial.println(val); 22 | } 23 | #endif 24 | 25 | motor_accel[0] = val; 26 | motor_accel[1] = val; 27 | } 28 | -------------------------------------------------------------------------------- /src/pot_motor_speed.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | extern void get_pot_motor_speed(int16_t * motor_accel, float angle, float angle_old, int16_t m1, int16_t m2); 4 | -------------------------------------------------------------------------------- /src/printf.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | void prints(char *fmt, ... ){ 5 | char buf[128] = {0}; // resulting string limited to 128 chars 6 | 7 | va_list args; 8 | va_start (args, fmt); 9 | uint8_t i = vsnprintf(buf, 128, fmt, args); 10 | buf[i]='\r'; 11 | 12 | va_end (args); 13 | Serial.print(buf); 14 | } 15 | 16 | void printsf(const char * tag, char * fmt, ...) { 17 | char buf[128] = {0}; // resulting string limited to 128 chars 18 | 19 | uint8_t n = sprintf(buf, "[%s] ", tag); 20 | 21 | va_list args; 22 | va_start (args, fmt); 23 | uint8_t i = vsnprintf(buf+n, 128, fmt, args); 24 | buf[i+n]='\r'; 25 | va_end (args); 26 | Serial.print(buf); 27 | } 28 | -------------------------------------------------------------------------------- /src/printf.h: -------------------------------------------------------------------------------- 1 | #ifndef PRINT_H 2 | #define PRINT_H 3 | 4 | 5 | extern void prints(char *fmt, ... ); 6 | extern void printsf(const char *, char * fmt, ...); 7 | #endif 8 | -------------------------------------------------------------------------------- /src/utils.h: -------------------------------------------------------------------------------- 1 | #define runEvery(t) for (static uint16_t _lasttime; (uint16_t)((uint16_t)millis() - _lasttime) >= (t); _lasttime += (t)) 2 | -------------------------------------------------------------------------------- /todo.txt: -------------------------------------------------------------------------------- 1 | enable wires to disable drivers when speed is zero (power saving) 2 | switch on and off 3 | better stabilization 4 | second PID to incorporate speed - move backwards and forward 5 | power arduino with same battery 6 | nice little cover 7 | --------------------------------------------------------------------------------