├── README.md └── Ball_Balance_MPU6050_DMP ├── keywords.txt ├── configure.h ├── helper_3dmath.h ├── I2Cdev.h ├── Ball_Balance_MPU6050_DMP.ino ├── MPU6050_6Axis_MotionApps20.h ├── MPU6050.h └── I2Cdev.cpp /README.md: -------------------------------------------------------------------------------- 1 | # UArm-Self-balancing-ball 2 | This is the original code for 2-wheel-self-balancing-robot. You can check the blog to see the mechanical parts of this: http://evol.net/blog/2-wheel-self-balancing-robot-by-using-arduino-and-mpu6050 3 | -------------------------------------------------------------------------------- /Ball_Balance_MPU6050_DMP/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 | -------------------------------------------------------------------------------- /Ball_Balance_MPU6050_DMP/configure.h: -------------------------------------------------------------------------------- 1 | #ifndef _CONFIGURE_H_ 2 | #define _CONFIGURE_H_ 3 | 4 | //************** motor ***************** 5 | #define MOTOR_DEADZONE 10 6 | #define POSITION_LIMIT 500 7 | #define PWM_LIMIT 255 8 | #define SAFETY_ANGLE 1 9 | //**************************************** 10 | 11 | //********** remote contorl ************ 12 | #define CLOCKWISE 0x01 13 | #define ANTICLOCKWISE 0x03 14 | #define FORWARD 0x02 15 | #define BACKWARD 0X08 16 | #define LEFT 0X04 17 | #define RIGHT 0X06 18 | //**************************************** 19 | 20 | //************* connect **************** 21 | #define LED 13 // pin_13 -> L-LED(on board) 22 | 23 | // L298 motor driver connected to the Arduino 2560 24 | #define MOTOR_1 6 // pin_6 -> 1-L298_ENA (timer4) 25 | #define MOTOR_2 7 // pin_7 -> 1-L298_ENB (timer4) 26 | #define MOTOR_3 8 // pin_8 -> 2-L298_ENB (timer4) 27 | #define DIR_M1_A 22 // pin_22 -> 1-L298_INA 28 | #define DIR_M1_B 23 // pin_23 -> 1-L298_INB 29 | #define DIR_M2_A 24 // pin_24 -> 1-L298_INC 30 | #define DIR_M2_B 25 // pin_25 -> 1-L298_IND 31 | #define DIR_M3_A 26 // pin_26 -> 2-L298_IND 32 | #define DIR_M3_B 27 // pin_27 -> 2-L298_INC 33 | 34 | // Motor encoder connected to the Arduino 2560 35 | // *The encoder signal lines are connected through A shaping with MCU 36 | #define SPD_INT_1 1 // pin_3 -> Motor_1_encoder_Yellow (external interruption 1) 37 | #define SPD_INT_2 4 // pin_19 -> Motor_2_encoder_Yellow (external interruption 4) 38 | #define SPD_INT_3 5 // pin_18 -> Motor_3_encoder_Yellow (external interruption 5) 39 | #define SPD_PUL_1 4 // pin_4 -> Motor_1_encoder_Orange 40 | #define SPD_PUL_2 5 // pin_5 -> Motor_2_encoder_Orange 41 | #define SPD_PUL_3 9 // pin_9 -> Motor_3_encoder_Orange 42 | 43 | // MPU6050 connected to the Arduino 2560 44 | #define SENSOR_INT 0 // pin_2 -> MPU6050_INT (external interruption 0) 45 | // pin_20 -> MPU6050_SDA (SDA) 46 | // pin_21 -> MPU6050_SCL (SCL) 47 | // Bluetooth connected to the Arduino 2560 48 | // pin_16 -> Bluetooth_RX (TX2) 49 | // pin_17 -> Bluetooth_TX (RX2) 50 | 51 | // By AD input K value, used for debugging 52 | #define K_AGL_AD A0 // pin_A0 -> (debug) 53 | #define K_AGL_DOT_AD A1 // pin_A1 -> (debug) 54 | #define K_POS_AD A2 // pin_A2 -> (debug) 55 | #define K_POS_DOT_AD A3 // pin_A3 -> (debug) 56 | 57 | // L298 motor driver connected to the motor 58 | // 1-L298_OUTA -> Motor_1_Black 59 | // 1-L298_OUTB -> Motor_1_Red 60 | // 1-L298_OUTC -> Motor_2_Grey 61 | // 1-L298_OUTD -> Motor_2_Red 62 | // 2-L298_OUTC -> Motor_3_Red 63 | // 2-L298_OUTD -> Motor_3_Black 64 | //**************************************** 65 | #endif -------------------------------------------------------------------------------- /Ball_Balance_MPU6050_DMP/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_ */ -------------------------------------------------------------------------------- /Ball_Balance_MPU6050_DMP/I2Cdev.h: -------------------------------------------------------------------------------- 1 | // I2Cdev library collection - Main I2C device class header file 2 | // Abstracts bit and byte I2C R/W functions into a convenient class 3 | // 6/9/2012 by Jeff Rowberg 4 | // 5 | // Changelog: 6 | // 2013-05-06 - add Francesco Ferrara's Fastwire v0.24 implementation with small modifications 7 | // 2013-05-05 - fix issue with writing bit values to words (Sasquatch/Farzanegan) 8 | // 2012-06-09 - fix major issue with reading > 32 bytes at a time with Arduino Wire 9 | // - add compiler warnings when using outdated or IDE or limited I2Cdev implementation 10 | // 2011-11-01 - fix write*Bits mask calculation (thanks sasquatch @ Arduino forums) 11 | // 2011-10-03 - added automatic Arduino version detection for ease of use 12 | // 2011-10-02 - added Gene Knight's NBWire TwoWire class implementation with small modifications 13 | // 2011-08-31 - added support for Arduino 1.0 Wire library (methods are different from 0.x) 14 | // 2011-08-03 - added optional timeout parameter to read* methods to easily change from default 15 | // 2011-08-02 - added support for 16-bit registers 16 | // - fixed incorrect Doxygen comments on some methods 17 | // - added timeout value for read operations (thanks mem @ Arduino forums) 18 | // 2011-07-30 - changed read/write function structures to return success or byte counts 19 | // - made all methods static for multi-device memory savings 20 | // 2011-07-28 - initial release 21 | 22 | /* ============================================ 23 | I2Cdev device library code is placed under the MIT license 24 | Copyright (c) 2013 Jeff Rowberg 25 | 26 | Permission is hereby granted, free of charge, to any person obtaining a copy 27 | of this software and associated documentation files (the "Software"), to deal 28 | in the Software without restriction, including without limitation the rights 29 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 30 | copies of the Software, and to permit persons to whom the Software is 31 | furnished to do so, subject to the following conditions: 32 | 33 | The above copyright notice and this permission notice shall be included in 34 | all copies or substantial portions of the Software. 35 | 36 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 37 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 38 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 39 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 40 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 41 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 42 | THE SOFTWARE. 43 | =============================================== 44 | */ 45 | 46 | #ifndef _I2CDEV_H_ 47 | #define _I2CDEV_H_ 48 | 49 | // ----------------------------------------------------------------------------- 50 | // I2C interface implementation setting 51 | // ----------------------------------------------------------------------------- 52 | #define I2CDEV_IMPLEMENTATION I2CDEV_ARDUINO_WIRE 53 | //#define I2CDEV_IMPLEMENTATION I2CDEV_BUILTIN_FASTWIRE 54 | 55 | // comment this out if you are using a non-optimal IDE/implementation setting 56 | // but want the compiler to shut up about it 57 | #define I2CDEV_IMPLEMENTATION_WARNINGS 58 | 59 | // ----------------------------------------------------------------------------- 60 | // I2C interface implementation options 61 | // ----------------------------------------------------------------------------- 62 | #define I2CDEV_ARDUINO_WIRE 1 // Wire object from Arduino 63 | #define I2CDEV_BUILTIN_NBWIRE 2 // Tweaked Wire object from Gene Knight's NBWire project 64 | // ^^^ NBWire implementation is still buggy w/some interrupts! 65 | #define I2CDEV_BUILTIN_FASTWIRE 3 // FastWire object from Francesco Ferrara's project 66 | #define I2CDEV_I2CMASTER_LIBRARY 4 // I2C object from DSSCircuits I2C-Master Library at https://github.com/DSSCircuits/I2C-Master-Library 67 | 68 | // ----------------------------------------------------------------------------- 69 | // Arduino-style "Serial.print" debug constant (uncomment to enable) 70 | // ----------------------------------------------------------------------------- 71 | //#define I2CDEV_SERIAL_DEBUG 72 | 73 | #ifdef ARDUINO 74 | #if ARDUINO < 100 75 | #include "WProgram.h" 76 | #else 77 | #include "Arduino.h" 78 | #endif 79 | #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE 80 | #include 81 | #endif 82 | #if I2CDEV_IMPLEMENTATION == I2CDEV_I2CMASTER_LIBRARY 83 | #include 84 | #endif 85 | #endif 86 | 87 | // 1000ms default read timeout (modify with "I2Cdev::readTimeout = [ms];") 88 | #define I2CDEV_DEFAULT_READ_TIMEOUT 1000 89 | 90 | class I2Cdev { 91 | public: 92 | I2Cdev(); 93 | 94 | static int8_t readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout); 95 | static int8_t readBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout); 96 | static int8_t readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout); 97 | static int8_t readBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout); 98 | static int8_t readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout); 99 | static int8_t readWord(uint8_t devAddr, uint8_t regAddr, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout); 100 | static int8_t readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout); 101 | static int8_t readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout); 102 | 103 | static bool writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data); 104 | static bool writeBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t data); 105 | static bool writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data); 106 | static bool writeBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t data); 107 | static bool writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data); 108 | static bool writeWord(uint8_t devAddr, uint8_t regAddr, uint16_t data); 109 | static bool writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data); 110 | static bool writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data); 111 | 112 | static uint16_t readTimeout; 113 | }; 114 | 115 | #if I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE 116 | ////////////////////// 117 | // FastWire 0.24 118 | // This is a library to help faster programs to read I2C devices. 119 | // Copyright(C) 2012 120 | // Francesco Ferrara 121 | ////////////////////// 122 | 123 | /* Master */ 124 | #define TW_START 0x08 125 | #define TW_REP_START 0x10 126 | 127 | /* Master Transmitter */ 128 | #define TW_MT_SLA_ACK 0x18 129 | #define TW_MT_SLA_NACK 0x20 130 | #define TW_MT_DATA_ACK 0x28 131 | #define TW_MT_DATA_NACK 0x30 132 | #define TW_MT_ARB_LOST 0x38 133 | 134 | /* Master Receiver */ 135 | #define TW_MR_ARB_LOST 0x38 136 | #define TW_MR_SLA_ACK 0x40 137 | #define TW_MR_SLA_NACK 0x48 138 | #define TW_MR_DATA_ACK 0x50 139 | #define TW_MR_DATA_NACK 0x58 140 | 141 | #define TW_OK 0 142 | #define TW_ERROR 1 143 | 144 | class Fastwire { 145 | private: 146 | static boolean waitInt(); 147 | 148 | public: 149 | static void setup(int khz, boolean pullup); 150 | static byte beginTransmission(byte device); 151 | static byte write(byte value); 152 | static byte writeBuf(byte device, byte address, byte *data, byte num); 153 | static byte readBuf(byte device, byte address, byte *data, byte num); 154 | static void reset(); 155 | static byte stop(); 156 | }; 157 | #endif 158 | 159 | #if I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE 160 | // NBWire implementation based heavily on code by Gene Knight 161 | // Originally posted on the Arduino forum at http://arduino.cc/forum/index.php/topic,70705.0.html 162 | // Originally offered to the i2cdevlib project at http://arduino.cc/forum/index.php/topic,68210.30.html 163 | 164 | #define NBWIRE_BUFFER_LENGTH 32 165 | 166 | class TwoWire { 167 | private: 168 | static uint8_t rxBuffer[]; 169 | static uint8_t rxBufferIndex; 170 | static uint8_t rxBufferLength; 171 | 172 | static uint8_t txAddress; 173 | static uint8_t txBuffer[]; 174 | static uint8_t txBufferIndex; 175 | static uint8_t txBufferLength; 176 | 177 | // static uint8_t transmitting; 178 | static void (*user_onRequest)(void); 179 | static void (*user_onReceive)(int); 180 | static void onRequestService(void); 181 | static void onReceiveService(uint8_t*, int); 182 | 183 | public: 184 | TwoWire(); 185 | void begin(); 186 | void begin(uint8_t); 187 | void begin(int); 188 | void beginTransmission(uint8_t); 189 | //void beginTransmission(int); 190 | uint8_t endTransmission(uint16_t timeout=0); 191 | void nbendTransmission(void (*function)(int)) ; 192 | uint8_t requestFrom(uint8_t, int, uint16_t timeout=0); 193 | //uint8_t requestFrom(int, int); 194 | void nbrequestFrom(uint8_t, int, void (*function)(int)); 195 | void send(uint8_t); 196 | void send(uint8_t*, uint8_t); 197 | //void send(int); 198 | void send(char*); 199 | uint8_t available(void); 200 | uint8_t receive(void); 201 | void onReceive(void (*)(int)); 202 | void onRequest(void (*)(void)); 203 | }; 204 | 205 | #define TWI_READY 0 206 | #define TWI_MRX 1 207 | #define TWI_MTX 2 208 | #define TWI_SRX 3 209 | #define TWI_STX 4 210 | 211 | #define TW_WRITE 0 212 | #define TW_READ 1 213 | 214 | #define TW_MT_SLA_NACK 0x20 215 | #define TW_MT_DATA_NACK 0x30 216 | 217 | #define CPU_FREQ 16000000L 218 | #define TWI_FREQ 100000L 219 | #define TWI_BUFFER_LENGTH 32 220 | 221 | /* TWI Status is in TWSR, in the top 5 bits: TWS7 - TWS3 */ 222 | 223 | #define TW_STATUS_MASK (_BV(TWS7)|_BV(TWS6)|_BV(TWS5)|_BV(TWS4)|_BV(TWS3)) 224 | #define TW_STATUS (TWSR & TW_STATUS_MASK) 225 | #define TW_START 0x08 226 | #define TW_REP_START 0x10 227 | #define TW_MT_SLA_ACK 0x18 228 | #define TW_MT_SLA_NACK 0x20 229 | #define TW_MT_DATA_ACK 0x28 230 | #define TW_MT_DATA_NACK 0x30 231 | #define TW_MT_ARB_LOST 0x38 232 | #define TW_MR_ARB_LOST 0x38 233 | #define TW_MR_SLA_ACK 0x40 234 | #define TW_MR_SLA_NACK 0x48 235 | #define TW_MR_DATA_ACK 0x50 236 | #define TW_MR_DATA_NACK 0x58 237 | #define TW_ST_SLA_ACK 0xA8 238 | #define TW_ST_ARB_LOST_SLA_ACK 0xB0 239 | #define TW_ST_DATA_ACK 0xB8 240 | #define TW_ST_DATA_NACK 0xC0 241 | #define TW_ST_LAST_DATA 0xC8 242 | #define TW_SR_SLA_ACK 0x60 243 | #define TW_SR_ARB_LOST_SLA_ACK 0x68 244 | #define TW_SR_GCALL_ACK 0x70 245 | #define TW_SR_ARB_LOST_GCALL_ACK 0x78 246 | #define TW_SR_DATA_ACK 0x80 247 | #define TW_SR_DATA_NACK 0x88 248 | #define TW_SR_GCALL_DATA_ACK 0x90 249 | #define TW_SR_GCALL_DATA_NACK 0x98 250 | #define TW_SR_STOP 0xA0 251 | #define TW_NO_INFO 0xF8 252 | #define TW_BUS_ERROR 0x00 253 | 254 | //#define _MMIO_BYTE(mem_addr) (*(volatile uint8_t *)(mem_addr)) 255 | //#define _SFR_BYTE(sfr) _MMIO_BYTE(_SFR_ADDR(sfr)) 256 | 257 | #ifndef sbi // set bit 258 | #define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit)) 259 | #endif // sbi 260 | 261 | #ifndef cbi // clear bit 262 | #define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit)) 263 | #endif // cbi 264 | 265 | extern TwoWire Wire; 266 | 267 | #endif // I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE 268 | 269 | #endif /* _I2CDEV_H_ */ 270 | -------------------------------------------------------------------------------- /Ball_Balance_MPU6050_DMP/Ball_Balance_MPU6050_DMP.ino: -------------------------------------------------------------------------------- 1 | #include "Wire.h" 2 | #include "I2Cdev.h" 3 | #include "configure.h" 4 | #include "MPU6050_6Axis_MotionApps20.h" 5 | 6 | MPU6050 mpu; // AD0 low = 0x68 7 | 8 | // MPU control/status vars 9 | bool dmpReady = false; // set true if DMP init was successful 10 | uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU 11 | uint8_t devStatus; // return status after each device operation (0 = success, !0 = error) 12 | uint16_t packetSize; // expected DMP packet size (default is 42 bytes) 13 | uint16_t fifoCount; // count of all bytes currently in FIFO 14 | uint8_t fifoBuffer[64]; // FIFO storage buffer 15 | 16 | // orientation/motion vars 17 | Quaternion q; // [w, x, y, z] quaternion container 18 | VectorFloat gravity; // [x, y, z] gravity vector 19 | int16_t gyro[3]; // [x, y, z] gyro vector 20 | float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector 21 | 22 | float K_angle_X, K_angular_rate_X, K_position_X, K_velocity_X; 23 | float K_angle_Y, K_angular_rate_Y, K_position_Y, K_velocity_Y; 24 | float position_X, velocity_X, position_Y, velocity_Y; 25 | float angle_Y, angular_rate_Y,angle_X, angular_rate_X; 26 | float gyro_X, acceler_X, gyro_Y, acceler_Y; 27 | float velocity_filter_X, velocity_filter_Y; 28 | float speed_X, speed_Y; 29 | volatile int speed_feedback_1, speed_feedback_2, speed_feedback_3; 30 | int motor1_output, motor2_output, motor3_output; 31 | int speed_feedback_X, speed_feedback_Y; 32 | int position_Need_X, position_Need_Y; 33 | char bufClr = 0; 34 | bool blinkState = false; 35 | 36 | volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high 37 | void dmpDataReady() 38 | { 39 | mpuInterrupt = true; 40 | } 41 | 42 | void setup() 43 | { 44 | init_data(); 45 | init_IO(); 46 | 47 | Wire.begin();// join I2C bus (I2Cdev library doesn't do this automatically) 48 | TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz) 49 | Serial.begin(115200); 50 | Serial2.begin(9600); 51 | // initialize device 52 | Serial.println(F("Initializing I2C devices...")); 53 | mpu.initialize(); 54 | 55 | // verify connection 56 | Serial.println(F("Testing device connections...")); 57 | Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed")); 58 | delay(2); 59 | 60 | // load and configure the DMP 61 | Serial.println(F("Initializing DMP...")); 62 | devStatus = mpu.dmpInitialize(); 63 | 64 | // supply your own gyro offsets here, scaled for min sensitivity 65 | mpu.setXGyroOffset(10); 66 | mpu.setYGyroOffset(-15); 67 | mpu.setZGyroOffset(14); 68 | mpu.setZAccelOffset(900); // 1688 factory default for my test chip 69 | 70 | // make sure it worked (returns 0 if so) 71 | if (devStatus == 0) 72 | { 73 | // turn on the DMP, now that it's ready 74 | Serial.println(F("Enabling DMP...")); 75 | mpu.setDMPEnabled(true); 76 | 77 | // enable Arduino interrupt detection 78 | Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)...")); 79 | attachInterrupt(0, dmpDataReady, RISING); 80 | mpuIntStatus = mpu.getIntStatus(); 81 | 82 | // set our DMP Ready flag so the main loop() function knows it's okay to use it 83 | Serial.println(F("DMP ready! Waiting for first interrupt...")); 84 | dmpReady = true; 85 | 86 | // get expected DMP packet size for later comparison 87 | packetSize = mpu.dmpGetFIFOPacketSize(); 88 | } 89 | else 90 | { 91 | // ERROR! 92 | // 1 = initial memory load failed 93 | // 2 = DMP configuration updates failed 94 | // (if it's going to break, usually the code will be 1) 95 | Serial.print(F("DMP Initialization failed (code ")); 96 | Serial.print(devStatus); 97 | Serial.println(F(")")); 98 | } 99 | } 100 | 101 | void loop() 102 | { 103 | // if programming failed, don't try to do anything 104 | if (!dmpReady) return; 105 | while (!mpuInterrupt && fifoCount < packetSize) {} 106 | // reset interrupt flag and get INT_STATUS byte 107 | mpuInterrupt = false; 108 | mpuIntStatus = mpu.getIntStatus(); 109 | 110 | // get current FIFO count 111 | fifoCount = mpu.getFIFOCount(); 112 | 113 | // check for overflow (this should never happen unless our code is too inefficient) 114 | if ((mpuIntStatus & 0x10) || fifoCount == 1024) 115 | { 116 | // reset so we can continue cleanly 117 | mpu.resetFIFO(); 118 | Serial.println(F("FIFO overflow!")); 119 | // otherwise, check for DMP data ready interrupt (this should happen frequently) 120 | } 121 | else if (mpuIntStatus & 0x02) 122 | { 123 | // wait for correct available data length, should be a VERY short wait 124 | while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount(); 125 | // read a packet from FIFO 126 | mpu.getFIFOBytes(fifoBuffer, packetSize); 127 | // track FIFO count here in case there is > 1 packet available 128 | // (this lets us immediately read more without waiting for an interrupt) 129 | fifoCount -= packetSize; 130 | //Get sensor data 131 | mpu.dmpGetQuaternion(&q, fifoBuffer); 132 | mpu.dmpGetGyro(gyro, fifoBuffer); 133 | mpu.dmpGetGravity(&gravity, &q); 134 | mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); 135 | 136 | // angle and angular rate unit: radian 137 | angle_X = ypr[2] + 0.005; // 0.017 is center of gravity offset 138 | angular_rate_X = -((double)gyro[0]/131.0); // converted to radian 139 | angle_Y = ypr[1] + 0.035; // 0.02 is center of gravity offset 140 | angular_rate_Y = -((double)gyro[1]/131.0); // converted to radian 141 | /* 142 | Serial.print("X: "); 143 | Serial.print(angle_X * RAD_TO_DEG);Serial.print(" "); 144 | Serial.print(angular_rate_X * RAD_TO_DEG);Serial.print(" "); 145 | Serial.print("Y: "); 146 | Serial.print(angle_Y * RAD_TO_DEG);Serial.print(" "); 147 | Serial.print(angular_rate_Y * RAD_TO_DEG);Serial.print(" "); 148 | Serial.print("\n"); 149 | */ 150 | threeWay_to_xoy(speed_feedback_1, speed_feedback_2, speed_feedback_3); 151 | XY_speed_calculate(); 152 | xoy_to_threeWay(speed_X, speed_Y); 153 | Wireless_Control(); //无线控制 154 | PWM_output(motor1_output, motor2_output, motor3_output); 155 | 156 | // blink LED to indicate activity 157 | blinkState = !blinkState; 158 | digitalWrite(LED, blinkState); 159 | } 160 | } 161 | 162 | void init_IO() 163 | { 164 | // configure I/O 165 | pinMode(LED, OUTPUT); 166 | pinMode(DIR_M1_A, OUTPUT);// 167 | pinMode(DIR_M1_B, OUTPUT);// 168 | pinMode(DIR_M2_A, OUTPUT);// 169 | pinMode(DIR_M2_B, OUTPUT); 170 | pinMode(DIR_M3_A, OUTPUT);// 171 | pinMode(DIR_M3_B, OUTPUT); 172 | pinMode(MOTOR_1, OUTPUT); 173 | pinMode(MOTOR_2, OUTPUT); 174 | pinMode(MOTOR_3, OUTPUT); 175 | pinMode(SPD_PUL_1, INPUT);// 176 | pinMode(SPD_PUL_2, INPUT);// 177 | pinMode(SPD_PUL_3, INPUT);// 178 | 179 | // configure external interruption 180 | attachInterrupt(SPD_INT_1, speed_int_1, RISING); 181 | attachInterrupt(SPD_INT_2, speed_int_2, RISING); 182 | attachInterrupt(SPD_INT_3, speed_int_3, RISING); 183 | } 184 | 185 | void init_data() 186 | { 187 | K_angle_X = 6.0928;// * 360.0; // 365.0 188 | K_angular_rate_X = 0.3584;// * 250.0; // 220.0 189 | K_position_X = 0.0011704;// * 100.0; // 90.0 190 | K_velocity_X = 0.159467;// * 270.0; // 265.0 191 | 192 | K_angle_Y = 6.0928;// * 360.0; //角度 193 | K_angular_rate_Y = 0.3584;// * 250.0; //角速度 194 | K_position_Y = 0.0011704;// * 100.0; //位移 195 | K_velocity_Y = 0.159467;// * 270.0; //速度 196 | } 197 | 198 | byte buf_tmp=0; 199 | void Wireless_Control() 200 | { 201 | position_Need_X = 0; 202 | position_Need_Y = 0; 203 | if(Serial2.available()) 204 | { 205 | buf_tmp=Serial2.read(); 206 | } 207 | switch(buf_tmp) 208 | { 209 | case FORWARD: position_Need_Y = 4;break; 210 | case BACKWARD: position_Need_Y = -4;break; 211 | case LEFT: position_Need_X = 4;break; 212 | case RIGHT: position_Need_X = -4;break; 213 | case CLOCKWISE: motor1_output -= 15; 214 | motor2_output -= 15; 215 | motor3_output -= 15; 216 | break; 217 | case ANTICLOCKWISE: motor1_output += 15; 218 | motor2_output += 15; 219 | motor3_output += 15; 220 | break; 221 | default:break; 222 | } 223 | 224 | if(++bufClr > 15) 225 | { 226 | buf_tmp=0; 227 | bufClr = 0; 228 | } 229 | } 230 | 231 | /* 232 | 任意方向直线运动 233 | 将车体坐标系xoy的速度转换成电机的控制坐标系中的速度 234 | 235 | 电机方向 全局运动坐标 传感器测量轴 236 | 237 | /\ | ^ | ^ 238 | V1 / \ V2 | Vy | | x | 239 | \/ | | | | 240 | <-- | ----> | ----> 241 | V3 | Vx | y 242 | */ 243 | void xoy_to_threeWay(float in_X, float in_Y) 244 | { 245 | motor1_output = in_X * 2 - (1.155 * in_Y); 246 | motor2_output = in_X * 2 + (1.155 * in_Y); 247 | motor3_output = -in_X * 2.6; // 2 is motor3 dead zone 248 | } 249 | 250 | void threeWay_to_xoy(int in_speed1, int in_speed2, int in_speed3) 251 | { 252 | speed_feedback_X = (in_speed3-(in_speed2+in_speed1)*0.5)*0.5; 253 | speed_feedback_Y = -0.866*(in_speed2-in_speed1); 254 | } 255 | 256 | //计算XY分量上的速度 257 | void XY_speed_calculate(void) 258 | { 259 | /* 260 | int K_angle_AD = analogRead(K_AGL_AD)-512; 261 | int K_angular_rate_AD = analogRead(K_AGL_DOT_AD)-512; 262 | int K_position_AD = analogRead(K_POS_AD)-512; 263 | int K_velocity_AD = analogRead(K_POS_DOT_AD)-512; 264 | */ 265 | /******** X轴 *********/ 266 | velocity_X = speed_feedback_X; //车轮的平均速度 267 | 268 | velocity_filter_X *= 0.95; //车速低通滤波 269 | velocity_filter_X += velocity_X * 0.05; 270 | 271 | position_X += velocity_filter_X; //车轮的位置 272 | position_X += position_Need_X; //期望位移 273 | 274 | if(position_X < -POSITION_LIMIT) //限幅，防止位置误差过大导致的不稳定 275 | position_X = -POSITION_LIMIT; 276 | else if(position_X > POSITION_LIMIT) 277 | position_X = POSITION_LIMIT; 278 | 279 | speed_X = K_angle_X * angle_X * 69 //69 280 | + K_angular_rate_X * angular_rate_X * (-124) // -124 281 | + K_position_X * position_X * 106 //106 282 | + K_velocity_X * velocity_filter_X * 90; //90 283 | 284 | /******** Y轴 *********/ 285 | velocity_Y = speed_feedback_Y; //车轮的平均速度 286 | 287 | velocity_filter_Y *= 0.95; //车速低通滤波 288 | velocity_filter_Y += velocity_Y * 0.05; 289 | 290 | position_Y += velocity_filter_Y; //车轮的位置 291 | position_Y += position_Need_Y; //期望位移 292 | 293 | if(position_Y < -POSITION_LIMIT) //限幅，防止位置误差过大导致的不稳定 294 | position_Y = -POSITION_LIMIT; 295 | else if(position_Y > POSITION_LIMIT) 296 | position_Y = POSITION_LIMIT; 297 | 298 | speed_Y = K_angle_Y * angle_Y * 163 //163 // * K_angle_AD 299 | + K_angular_rate_Y * angular_rate_Y * 107 //107 // * K_angular_rate_AD 300 | + K_position_Y * position_Y * 130 //130 // * K_position_AD 301 | + K_velocity_Y * velocity_filter_Y * 75; //75 // * K_velocity_AD 302 | 303 | /* 304 | Serial.print(K_angle_AD);Serial.print(" "); 305 | Serial.print(K_angular_rate_AD);Serial.print(" "); 306 | Serial.print(K_position_AD);Serial.print(" "); 307 | Serial.print(K_velocity_AD);Serial.print(" "); 308 | Serial.print("\n"); 309 | */ 310 | /* 311 | Serial.print(motor1_output);Serial.print(" "); 312 | Serial.print(motor2_output);Serial.print(" "); 313 | Serial.print(motor3_output);Serial.print(" "); 314 | 315 | Serial.print(speed_feedback_1);Serial.print(" "); 316 | Serial.print(speed_feedback_2);Serial.print(" "); 317 | Serial.print(speed_feedback_3);Serial.print(" "); 318 | 319 | Serial.print(speed_X);Serial.print(" "); 320 | Serial.print(speed_Y);Serial.print(" "); 321 | 322 | Serial.print(speed_feedback_X);Serial.print(" "); 323 | Serial.print(speed_feedback_Y);Serial.print(" "); 324 | 325 | Serial.print("\n"); 326 | */ 327 | speed_feedback_1 = 0; 328 | speed_feedback_2 = 0; 329 | speed_feedback_3 = 0; 330 | } 331 | 332 | void PWM_output(int pwm_1,int pwm_2,int pwm_3) 333 | { 334 | // PWM1 335 | if(pwm_1 < 0) 336 | { 337 | digitalWrite(DIR_M1_A, LOW); 338 | digitalWrite(DIR_M1_B, HIGH); 339 | pwm_1 = -pwm_1; 340 | } 341 | else 342 | { 343 | digitalWrite(DIR_M1_A, HIGH); 344 | digitalWrite(DIR_M1_B, LOW); 345 | } 346 | //PWM2 347 | if(pwm_2 < 0) 348 | { 349 | digitalWrite(DIR_M2_A, LOW); 350 | digitalWrite(DIR_M2_B, HIGH); 351 | pwm_2 = -pwm_2; 352 | } 353 | else 354 | { 355 | digitalWrite(DIR_M2_A, HIGH); 356 | digitalWrite(DIR_M2_B, LOW); 357 | } 358 | //PWM3 359 | if(pwm_3 < 0) 360 | { 361 | digitalWrite(DIR_M3_A, LOW); 362 | digitalWrite(DIR_M3_B, HIGH); 363 | pwm_3 = -pwm_3; 364 | } 365 | else 366 | { 367 | digitalWrite(DIR_M3_A, HIGH); 368 | digitalWrite(DIR_M3_B, LOW); 369 | } 370 | 371 | pwm_1 += MOTOR_DEADZONE; 372 | pwm_2 += MOTOR_DEADZONE; 373 | pwm_3 += MOTOR_DEADZONE; 374 | 375 | /* 376 | if(abs(angle_X)>0.7 || abs(angle_Y)>0.7) 377 | { 378 | pwm_1 = pwm_2 = pwm_3 = 0; 379 | } 380 | */ 381 | analogWrite(MOTOR_1, pwm_1>PWM_LIMIT? PWM_LIMIT:pwm_1); // Pin6 Motor 1 382 | analogWrite(MOTOR_2, pwm_2>PWM_LIMIT? PWM_LIMIT:pwm_2); // Pin7 Motor 2 383 | analogWrite(MOTOR_3, pwm_3>PWM_LIMIT? PWM_LIMIT:pwm_3); // Pin8 Motor 3 384 | } 385 | 386 | void speed_int_1() 387 | { 388 | if(digitalRead(SPD_PUL_1)) speed_feedback_1+=1; 389 | else speed_feedback_1-=1; 390 | } 391 | 392 | void speed_int_2() 393 | { 394 | if(digitalRead(SPD_PUL_2)) speed_feedback_2+=1; 395 | else speed_feedback_2-=1; 396 | } 397 | 398 | void speed_int_3() 399 | { 400 | if(digitalRead(SPD_PUL_3)) speed_feedback_3+=1; 401 | else speed_feedback_3-=1; 402 | } 403 | 404 | -------------------------------------------------------------------------------- /Ball_Balance_MPU6050_DMP/MPU6050_6Axis_MotionApps20.h: -------------------------------------------------------------------------------- 1 | // I2Cdev library collection - MPU6050 I2C device class, 6-axis MotionApps 2.0 implementation 2 | // Based on InvenSense MPU-6050 register map document rev. 2.0, 5/19/2011 (RM-MPU-6000A-00) 3 | // 5/20/2013 by Jeff Rowberg 4 | // Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib 5 | // 6 | // Changelog: 7 | // ... - ongoing debug release 8 | 9 | /* ============================================ 10 | I2Cdev device library code is placed under the MIT license 11 | Copyright (c) 2012 Jeff Rowberg 12 | 13 | Permission is hereby granted, free of charge, to any person obtaining a copy 14 | of this software and associated documentation files (the "Software"), to deal 15 | in the Software without restriction, including without limitation the rights 16 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 17 | copies of the Software, and to permit persons to whom the Software is 18 | furnished to do so, subject to the following conditions: 19 | 20 | The above copyright notice and this permission notice shall be included in 21 | all copies or substantial portions of the Software. 22 | 23 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 24 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 25 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 26 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 27 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 28 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 29 | THE SOFTWARE. 30 | =============================================== 31 | */ 32 | 33 | #ifndef _MPU6050_6AXIS_MOTIONAPPS20_H_ 34 | #define _MPU6050_6AXIS_MOTIONAPPS20_H_ 35 | 36 | #include "I2Cdev.h" 37 | #include "helper_3dmath.h" 38 | 39 | // MotionApps 2.0 DMP implementation, built using the MPU-6050EVB evaluation board 40 | #define MPU6050_INCLUDE_DMP_MOTIONAPPS20 41 | 42 | #include "MPU6050.h" 43 | 44 | // Tom Carpenter's conditional PROGMEM code 45 | // http://forum.arduino.cc/index.php?topic=129407.0 46 | #ifndef __arm__ 47 | #include 48 | #else 49 | // Teensy 3.0 library conditional PROGMEM code from Paul Stoffregen 50 | #ifndef __PGMSPACE_H_ 51 | #define __PGMSPACE_H_ 1 52 | #include 53 | 54 | #define PROGMEM 55 | #define PGM_P const char * 56 | #define PSTR(str) (str) 57 | #define F(x) x 58 | 59 | typedef void prog_void; 60 | typedef char prog_char; 61 | typedef unsigned char prog_uchar; 62 | typedef int8_t prog_int8_t; 63 | typedef uint8_t prog_uint8_t; 64 | typedef int16_t prog_int16_t; 65 | typedef uint16_t prog_uint16_t; 66 | typedef int32_t prog_int32_t; 67 | typedef uint32_t prog_uint32_t; 68 | 69 | #define strcpy_P(dest, src) strcpy((dest), (src)) 70 | #define strcat_P(dest, src) strcat((dest), (src)) 71 | #define strcmp_P(a, b) strcmp((a), (b)) 72 | 73 | #define pgm_read_byte(addr) (*(const unsigned char *)(addr)) 74 | #define pgm_read_word(addr) (*(const unsigned short *)(addr)) 75 | #define pgm_read_dword(addr) (*(const unsigned long *)(addr)) 76 | #define pgm_read_float(addr) (*(const float *)(addr)) 77 | 78 | #define pgm_read_byte_near(addr) pgm_read_byte(addr) 79 | #define pgm_read_word_near(addr) pgm_read_word(addr) 80 | #define pgm_read_dword_near(addr) pgm_read_dword(addr) 81 | #define pgm_read_float_near(addr) pgm_read_float(addr) 82 | #define pgm_read_byte_far(addr) pgm_read_byte(addr) 83 | #define pgm_read_word_far(addr) pgm_read_word(addr) 84 | #define pgm_read_dword_far(addr) pgm_read_dword(addr) 85 | #define pgm_read_float_far(addr) pgm_read_float(addr) 86 | #endif 87 | #endif 88 | 89 | /* Source is from the InvenSense MotionApps v2 demo code. Original source is 90 | * unavailable, unless you happen to be amazing as decompiling binary by 91 | * hand (in which case, please contact me, and I'm totally serious). 92 | * 93 | * Also, I'd like to offer many, many thanks to Noah Zerkin for all of the 94 | * DMP reverse-engineering he did to help make this bit of wizardry 95 | * possible. 96 | */ 97 | 98 | // NOTE! Enabling DEBUG adds about 3.3kB to the flash program size. 99 | // Debug output is now working even on ATMega328P MCUs (e.g. Arduino Uno) 100 | // after moving string constants to flash memory storage using the F() 101 | // compiler macro (Arduino IDE 1.0+ required). 102 | 103 | //#define DEBUG 104 | #ifdef DEBUG 105 | #define DEBUG_PRINT(x) Serial.print(x) 106 | #define DEBUG_PRINTF(x, y) Serial.print(x, y) 107 | #define DEBUG_PRINTLN(x) Serial.println(x) 108 | #define DEBUG_PRINTLNF(x, y) Serial.println(x, y) 109 | #else 110 | #define DEBUG_PRINT(x) 111 | #define DEBUG_PRINTF(x, y) 112 | #define DEBUG_PRINTLN(x) 113 | #define DEBUG_PRINTLNF(x, y) 114 | #endif 115 | 116 | #define MPU6050_DMP_CODE_SIZE 1929 // dmpMemory[] 117 | #define MPU6050_DMP_CONFIG_SIZE 192 // dmpConfig[] 118 | #define MPU6050_DMP_UPDATES_SIZE 47 // dmpUpdates[] 119 | 120 | /* ================================================================================================ * 121 | | Default MotionApps v2.0 42-byte FIFO packet structure: | 122 | | | 123 | | [QUAT W][ ][QUAT X][ ][QUAT Y][ ][QUAT Z][ ][GYRO X][ ][GYRO Y][ ] | 124 | | 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 | 125 | | | 126 | | [GYRO Z][ ][ACC X ][ ][ACC Y ][ ][ACC Z ][ ][ ] | 127 | | 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 | 128 | * ================================================================================================ */ 129 | 130 | // this block of memory gets written to the MPU on start-up, and it seems 131 | // to be volatile memory, so it has to be done each time (it only takes ~1 132 | // second though) 133 | const unsigned char dmpMemory[MPU6050_DMP_CODE_SIZE] PROGMEM = { 134 | // bank 0, 256 bytes 135 | 0xFB, 0x00, 0x00, 0x3E, 0x00, 0x0B, 0x00, 0x36, 0x00, 0x01, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00, 136 | 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0xFA, 0x80, 0x00, 0x0B, 0x12, 0x82, 0x00, 0x01, 137 | 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 138 | 0x00, 0x28, 0x00, 0x00, 0xFF, 0xFF, 0x45, 0x81, 0xFF, 0xFF, 0xFA, 0x72, 0x00, 0x00, 0x00, 0x00, 139 | 0x00, 0x00, 0x03, 0xE8, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x7F, 0xFF, 0xFF, 0xFE, 0x80, 0x01, 140 | 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 141 | 0x00, 0x3E, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xCA, 0xE3, 0x09, 0x3E, 0x80, 0x00, 0x00, 142 | 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00, 143 | 0x41, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x0B, 0x2A, 0x00, 0x00, 0x16, 0x55, 0x00, 0x00, 0x21, 0x82, 144 | 0xFD, 0x87, 0x26, 0x50, 0xFD, 0x80, 0x00, 0x00, 0x00, 0x1F, 0x00, 0x00, 0x00, 0x05, 0x80, 0x00, 145 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 146 | 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x6F, 0x00, 0x02, 0x65, 0x32, 0x00, 0x00, 0x5E, 0xC0, 147 | 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 148 | 0xFB, 0x8C, 0x6F, 0x5D, 0xFD, 0x5D, 0x08, 0xD9, 0x00, 0x7C, 0x73, 0x3B, 0x00, 0x6C, 0x12, 0xCC, 149 | 0x32, 0x00, 0x13, 0x9D, 0x32, 0x00, 0xD0, 0xD6, 0x32, 0x00, 0x08, 0x00, 0x40, 0x00, 0x01, 0xF4, 150 | 0xFF, 0xE6, 0x80, 0x79, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0xD0, 0xD6, 0x00, 0x00, 0x27, 0x10, 151 | 152 | // bank 1, 256 bytes 153 | 0xFB, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 154 | 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x00, 155 | 0x00, 0x00, 0xFA, 0x36, 0xFF, 0xBC, 0x30, 0x8E, 0x00, 0x05, 0xFB, 0xF0, 0xFF, 0xD9, 0x5B, 0xC8, 156 | 0xFF, 0xD0, 0x9A, 0xBE, 0x00, 0x00, 0x10, 0xA9, 0xFF, 0xF4, 0x1E, 0xB2, 0x00, 0xCE, 0xBB, 0xF7, 157 | 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x04, 0x00, 0x02, 0x00, 0x02, 0x02, 0x00, 0x00, 0x0C, 158 | 0xFF, 0xC2, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0xCF, 0x80, 0x00, 0x40, 0x00, 0x00, 0x00, 159 | 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14, 160 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 161 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 162 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 163 | 0x00, 0x00, 0x00, 0x00, 0x03, 0x3F, 0x68, 0xB6, 0x79, 0x35, 0x28, 0xBC, 0xC6, 0x7E, 0xD1, 0x6C, 164 | 0x80, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0xB2, 0x6A, 0x00, 0x00, 0x00, 0x00, 165 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3F, 0xF0, 0x00, 0x00, 0x00, 0x30, 166 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 167 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 168 | 0x00, 0x00, 0x25, 0x4D, 0x00, 0x2F, 0x70, 0x6D, 0x00, 0x00, 0x05, 0xAE, 0x00, 0x0C, 0x02, 0xD0, 169 | 170 | // bank 2, 256 bytes 171 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 172 | 0x00, 0x00, 0x01, 0x00, 0x00, 0x44, 0x00, 0x00, 0x00, 0x00, 0x0C, 0x00, 0x00, 0x00, 0x01, 0x00, 173 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x54, 0x00, 0x00, 0xFF, 0xEF, 0x00, 0x00, 174 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 175 | 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 176 | 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 177 | 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 178 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 179 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 180 | 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 181 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 182 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 183 | 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 184 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 185 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 186 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 187 | 188 | // bank 3, 256 bytes 189 | 0xD8, 0xDC, 0xBA, 0xA2, 0xF1, 0xDE, 0xB2, 0xB8, 0xB4, 0xA8, 0x81, 0x91, 0xF7, 0x4A, 0x90, 0x7F, 190 | 0x91, 0x6A, 0xF3, 0xF9, 0xDB, 0xA8, 0xF9, 0xB0, 0xBA, 0xA0, 0x80, 0xF2, 0xCE, 0x81, 0xF3, 0xC2, 191 | 0xF1, 0xC1, 0xF2, 0xC3, 0xF3, 0xCC, 0xA2, 0xB2, 0x80, 0xF1, 0xC6, 0xD8, 0x80, 0xBA, 0xA7, 0xDF, 192 | 0xDF, 0xDF, 0xF2, 0xA7, 0xC3, 0xCB, 0xC5, 0xB6, 0xF0, 0x87, 0xA2, 0x94, 0x24, 0x48, 0x70, 0x3C, 193 | 0x95, 0x40, 0x68, 0x34, 0x58, 0x9B, 0x78, 0xA2, 0xF1, 0x83, 0x92, 0x2D, 0x55, 0x7D, 0xD8, 0xB1, 194 | 0xB4, 0xB8, 0xA1, 0xD0, 0x91, 0x80, 0xF2, 0x70, 0xF3, 0x70, 0xF2, 0x7C, 0x80, 0xA8, 0xF1, 0x01, 195 | 0xB0, 0x98, 0x87, 0xD9, 0x43, 0xD8, 0x86, 0xC9, 0x88, 0xBA, 0xA1, 0xF2, 0x0E, 0xB8, 0x97, 0x80, 196 | 0xF1, 0xA9, 0xDF, 0xDF, 0xDF, 0xAA, 0xDF, 0xDF, 0xDF, 0xF2, 0xAA, 0xC5, 0xCD, 0xC7, 0xA9, 0x0C, 197 | 0xC9, 0x2C, 0x97, 0x97, 0x97, 0x97, 0xF1, 0xA9, 0x89, 0x26, 0x46, 0x66, 0xB0, 0xB4, 0xBA, 0x80, 198 | 0xAC, 0xDE, 0xF2, 0xCA, 0xF1, 0xB2, 0x8C, 0x02, 0xA9, 0xB6, 0x98, 0x00, 0x89, 0x0E, 0x16, 0x1E, 199 | 0xB8, 0xA9, 0xB4, 0x99, 0x2C, 0x54, 0x7C, 0xB0, 0x8A, 0xA8, 0x96, 0x36, 0x56, 0x76, 0xF1, 0xB9, 200 | 0xAF, 0xB4, 0xB0, 0x83, 0xC0, 0xB8, 0xA8, 0x97, 0x11, 0xB1, 0x8F, 0x98, 0xB9, 0xAF, 0xF0, 0x24, 201 | 0x08, 0x44, 0x10, 0x64, 0x18, 0xF1, 0xA3, 0x29, 0x55, 0x7D, 0xAF, 0x83, 0xB5, 0x93, 0xAF, 0xF0, 202 | 0x00, 0x28, 0x50, 0xF1, 0xA3, 0x86, 0x9F, 0x61, 0xA6, 0xDA, 0xDE, 0xDF, 0xD9, 0xFA, 0xA3, 0x86, 203 | 0x96, 0xDB, 0x31, 0xA6, 0xD9, 0xF8, 0xDF, 0xBA, 0xA6, 0x8F, 0xC2, 0xC5, 0xC7, 0xB2, 0x8C, 0xC1, 204 | 0xB8, 0xA2, 0xDF, 0xDF, 0xDF, 0xA3, 0xDF, 0xDF, 0xDF, 0xD8, 0xD8, 0xF1, 0xB8, 0xA8, 0xB2, 0x86, 205 | 206 | // bank 4, 256 bytes 207 | 0xB4, 0x98, 0x0D, 0x35, 0x5D, 0xB8, 0xAA, 0x98, 0xB0, 0x87, 0x2D, 0x35, 0x3D, 0xB2, 0xB6, 0xBA, 208 | 0xAF, 0x8C, 0x96, 0x19, 0x8F, 0x9F, 0xA7, 0x0E, 0x16, 0x1E, 0xB4, 0x9A, 0xB8, 0xAA, 0x87, 0x2C, 209 | 0x54, 0x7C, 0xB9, 0xA3, 0xDE, 0xDF, 0xDF, 0xA3, 0xB1, 0x80, 0xF2, 0xC4, 0xCD, 0xC9, 0xF1, 0xB8, 210 | 0xA9, 0xB4, 0x99, 0x83, 0x0D, 0x35, 0x5D, 0x89, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0xB5, 0x93, 0xA3, 211 | 0x0E, 0x16, 0x1E, 0xA9, 0x2C, 0x54, 0x7C, 0xB8, 0xB4, 0xB0, 0xF1, 0x97, 0x83, 0xA8, 0x11, 0x84, 212 | 0xA5, 0x09, 0x98, 0xA3, 0x83, 0xF0, 0xDA, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xD8, 0xF1, 0xA5, 213 | 0x29, 0x55, 0x7D, 0xA5, 0x85, 0x95, 0x02, 0x1A, 0x2E, 0x3A, 0x56, 0x5A, 0x40, 0x48, 0xF9, 0xF3, 214 | 0xA3, 0xD9, 0xF8, 0xF0, 0x98, 0x83, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0x97, 0x82, 0xA8, 0xF1, 215 | 0x11, 0xF0, 0x98, 0xA2, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xDA, 0xF3, 0xDE, 0xD8, 0x83, 0xA5, 216 | 0x94, 0x01, 0xD9, 0xA3, 0x02, 0xF1, 0xA2, 0xC3, 0xC5, 0xC7, 0xD8, 0xF1, 0x84, 0x92, 0xA2, 0x4D, 217 | 0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9, 218 | 0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0x93, 0xA3, 0x4D, 219 | 0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9, 220 | 0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0xA8, 0x8A, 0x9A, 221 | 0xF0, 0x28, 0x50, 0x78, 0x9E, 0xF3, 0x88, 0x18, 0xF1, 0x9F, 0x1D, 0x98, 0xA8, 0xD9, 0x08, 0xD8, 222 | 0xC8, 0x9F, 0x12, 0x9E, 0xF3, 0x15, 0xA8, 0xDA, 0x12, 0x10, 0xD8, 0xF1, 0xAF, 0xC8, 0x97, 0x87, 223 | 224 | // bank 5, 256 bytes 225 | 0x34, 0xB5, 0xB9, 0x94, 0xA4, 0x21, 0xF3, 0xD9, 0x22, 0xD8, 0xF2, 0x2D, 0xF3, 0xD9, 0x2A, 0xD8, 226 | 0xF2, 0x35, 0xF3, 0xD9, 0x32, 0xD8, 0x81, 0xA4, 0x60, 0x60, 0x61, 0xD9, 0x61, 0xD8, 0x6C, 0x68, 227 | 0x69, 0xD9, 0x69, 0xD8, 0x74, 0x70, 0x71, 0xD9, 0x71, 0xD8, 0xB1, 0xA3, 0x84, 0x19, 0x3D, 0x5D, 228 | 0xA3, 0x83, 0x1A, 0x3E, 0x5E, 0x93, 0x10, 0x30, 0x81, 0x10, 0x11, 0xB8, 0xB0, 0xAF, 0x8F, 0x94, 229 | 0xF2, 0xDA, 0x3E, 0xD8, 0xB4, 0x9A, 0xA8, 0x87, 0x29, 0xDA, 0xF8, 0xD8, 0x87, 0x9A, 0x35, 0xDA, 230 | 0xF8, 0xD8, 0x87, 0x9A, 0x3D, 0xDA, 0xF8, 0xD8, 0xB1, 0xB9, 0xA4, 0x98, 0x85, 0x02, 0x2E, 0x56, 231 | 0xA5, 0x81, 0x00, 0x0C, 0x14, 0xA3, 0x97, 0xB0, 0x8A, 0xF1, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9, 232 | 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x84, 0x0D, 0xDA, 0x0E, 0xD8, 0xA3, 0x29, 0x83, 0xDA, 233 | 0x2C, 0x0E, 0xD8, 0xA3, 0x84, 0x49, 0x83, 0xDA, 0x2C, 0x4C, 0x0E, 0xD8, 0xB8, 0xB0, 0xA8, 0x8A, 234 | 0x9A, 0xF5, 0x20, 0xAA, 0xDA, 0xDF, 0xD8, 0xA8, 0x40, 0xAA, 0xD0, 0xDA, 0xDE, 0xD8, 0xA8, 0x60, 235 | 0xAA, 0xDA, 0xD0, 0xDF, 0xD8, 0xF1, 0x97, 0x86, 0xA8, 0x31, 0x9B, 0x06, 0x99, 0x07, 0xAB, 0x97, 236 | 0x28, 0x88, 0x9B, 0xF0, 0x0C, 0x20, 0x14, 0x40, 0xB8, 0xB0, 0xB4, 0xA8, 0x8C, 0x9C, 0xF0, 0x04, 237 | 0x28, 0x51, 0x79, 0x1D, 0x30, 0x14, 0x38, 0xB2, 0x82, 0xAB, 0xD0, 0x98, 0x2C, 0x50, 0x50, 0x78, 238 | 0x78, 0x9B, 0xF1, 0x1A, 0xB0, 0xF0, 0x8A, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x8B, 0x29, 0x51, 0x79, 239 | 0x8A, 0x24, 0x70, 0x59, 0x8B, 0x20, 0x58, 0x71, 0x8A, 0x44, 0x69, 0x38, 0x8B, 0x39, 0x40, 0x68, 240 | 0x8A, 0x64, 0x48, 0x31, 0x8B, 0x30, 0x49, 0x60, 0xA5, 0x88, 0x20, 0x09, 0x71, 0x58, 0x44, 0x68, 241 | 242 | // bank 6, 256 bytes 243 | 0x11, 0x39, 0x64, 0x49, 0x30, 0x19, 0xF1, 0xAC, 0x00, 0x2C, 0x54, 0x7C, 0xF0, 0x8C, 0xA8, 0x04, 244 | 0x28, 0x50, 0x78, 0xF1, 0x88, 0x97, 0x26, 0xA8, 0x59, 0x98, 0xAC, 0x8C, 0x02, 0x26, 0x46, 0x66, 245 | 0xF0, 0x89, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31, 246 | 0xA9, 0x88, 0x09, 0x20, 0x59, 0x70, 0xAB, 0x11, 0x38, 0x40, 0x69, 0xA8, 0x19, 0x31, 0x48, 0x60, 247 | 0x8C, 0xA8, 0x3C, 0x41, 0x5C, 0x20, 0x7C, 0x00, 0xF1, 0x87, 0x98, 0x19, 0x86, 0xA8, 0x6E, 0x76, 248 | 0x7E, 0xA9, 0x99, 0x88, 0x2D, 0x55, 0x7D, 0x9E, 0xB9, 0xA3, 0x8A, 0x22, 0x8A, 0x6E, 0x8A, 0x56, 249 | 0x8A, 0x5E, 0x9F, 0xB1, 0x83, 0x06, 0x26, 0x46, 0x66, 0x0E, 0x2E, 0x4E, 0x6E, 0x9D, 0xB8, 0xAD, 250 | 0x00, 0x2C, 0x54, 0x7C, 0xF2, 0xB1, 0x8C, 0xB4, 0x99, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0x81, 0x91, 251 | 0xAC, 0x38, 0xAD, 0x3A, 0xB5, 0x83, 0x91, 0xAC, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9, 0x48, 0xD8, 252 | 0x6D, 0xD9, 0x68, 0xD8, 0x8C, 0x9D, 0xAE, 0x29, 0xD9, 0x04, 0xAE, 0xD8, 0x51, 0xD9, 0x04, 0xAE, 253 | 0xD8, 0x79, 0xD9, 0x04, 0xD8, 0x81, 0xF3, 0x9D, 0xAD, 0x00, 0x8D, 0xAE, 0x19, 0x81, 0xAD, 0xD9, 254 | 0x01, 0xD8, 0xF2, 0xAE, 0xDA, 0x26, 0xD8, 0x8E, 0x91, 0x29, 0x83, 0xA7, 0xD9, 0xAD, 0xAD, 0xAD, 255 | 0xAD, 0xF3, 0x2A, 0xD8, 0xD8, 0xF1, 0xB0, 0xAC, 0x89, 0x91, 0x3E, 0x5E, 0x76, 0xF3, 0xAC, 0x2E, 256 | 0x2E, 0xF1, 0xB1, 0x8C, 0x5A, 0x9C, 0xAC, 0x2C, 0x28, 0x28, 0x28, 0x9C, 0xAC, 0x30, 0x18, 0xA8, 257 | 0x98, 0x81, 0x28, 0x34, 0x3C, 0x97, 0x24, 0xA7, 0x28, 0x34, 0x3C, 0x9C, 0x24, 0xF2, 0xB0, 0x89, 258 | 0xAC, 0x91, 0x2C, 0x4C, 0x6C, 0x8A, 0x9B, 0x2D, 0xD9, 0xD8, 0xD8, 0x51, 0xD9, 0xD8, 0xD8, 0x79, 259 | 260 | // bank 7, 138 bytes (remainder) 261 | 0xD9, 0xD8, 0xD8, 0xF1, 0x9E, 0x88, 0xA3, 0x31, 0xDA, 0xD8, 0xD8, 0x91, 0x2D, 0xD9, 0x28, 0xD8, 262 | 0x4D, 0xD9, 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x83, 0x93, 0x35, 0x3D, 0x80, 0x25, 0xDA, 263 | 0xD8, 0xD8, 0x85, 0x69, 0xDA, 0xD8, 0xD8, 0xB4, 0x93, 0x81, 0xA3, 0x28, 0x34, 0x3C, 0xF3, 0xAB, 264 | 0x8B, 0xF8, 0xA3, 0x91, 0xB6, 0x09, 0xB4, 0xD9, 0xAB, 0xDE, 0xFA, 0xB0, 0x87, 0x9C, 0xB9, 0xA3, 265 | 0xDD, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x95, 0xF1, 0xA3, 0xA3, 0xA3, 0x9D, 0xF1, 0xA3, 0xA3, 0xA3, 266 | 0xA3, 0xF2, 0xA3, 0xB4, 0x90, 0x80, 0xF2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 267 | 0xA3, 0xB2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xB0, 0x87, 0xB5, 0x99, 0xF1, 0xA3, 0xA3, 0xA3, 268 | 0x98, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x97, 0xA3, 0xA3, 0xA3, 0xA3, 0xF3, 0x9B, 0xA3, 0xA3, 0xDC, 269 | 0xB9, 0xA7, 0xF1, 0x26, 0x26, 0x26, 0xD8, 0xD8, 0xFF 270 | }; 271 | 272 | // thanks to Noah Zerkin for piecing this stuff together! 273 | const unsigned char dmpConfig[MPU6050_DMP_CONFIG_SIZE] PROGMEM = { 274 | // BANK OFFSET LENGTH [DATA] 275 | 0x03, 0x7B, 0x03, 0x4C, 0xCD, 0x6C, // FCFG_1 inv_set_gyro_calibration 276 | 0x03, 0xAB, 0x03, 0x36, 0x56, 0x76, // FCFG_3 inv_set_gyro_calibration 277 | 0x00, 0x68, 0x04, 0x02, 0xCB, 0x47, 0xA2, // D_0_104 inv_set_gyro_calibration 278 | 0x02, 0x18, 0x04, 0x00, 0x05, 0x8B, 0xC1, // D_0_24 inv_set_gyro_calibration 279 | 0x01, 0x0C, 0x04, 0x00, 0x00, 0x00, 0x00, // D_1_152 inv_set_accel_calibration 280 | 0x03, 0x7F, 0x06, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97, // FCFG_2 inv_set_accel_calibration 281 | 0x03, 0x89, 0x03, 0x26, 0x46, 0x66, // FCFG_7 inv_set_accel_calibration 282 | 0x00, 0x6C, 0x02, 0x20, 0x00, // D_0_108 inv_set_accel_calibration 283 | 0x02, 0x40, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_00 inv_set_compass_calibration 284 | 0x02, 0x44, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_01 285 | 0x02, 0x48, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_02 286 | 0x02, 0x4C, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_10 287 | 0x02, 0x50, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_11 288 | 0x02, 0x54, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_12 289 | 0x02, 0x58, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_20 290 | 0x02, 0x5C, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_21 291 | 0x02, 0xBC, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_22 292 | 0x01, 0xEC, 0x04, 0x00, 0x00, 0x40, 0x00, // D_1_236 inv_apply_endian_accel 293 | 0x03, 0x7F, 0x06, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97, // FCFG_2 inv_set_mpu_sensors 294 | 0x04, 0x02, 0x03, 0x0D, 0x35, 0x5D, // CFG_MOTION_BIAS inv_turn_on_bias_from_no_motion 295 | 0x04, 0x09, 0x04, 0x87, 0x2D, 0x35, 0x3D, // FCFG_5 inv_set_bias_update 296 | 0x00, 0xA3, 0x01, 0x00, // D_0_163 inv_set_dead_zone 297 | // SPECIAL 0x01 = enable interrupts 298 | 0x00, 0x00, 0x00, 0x01, // SET INT_ENABLE at i=22, SPECIAL INSTRUCTION 299 | 0x07, 0x86, 0x01, 0xFE, // CFG_6 inv_set_fifo_interupt 300 | 0x07, 0x41, 0x05, 0xF1, 0x20, 0x28, 0x30, 0x38, // CFG_8 inv_send_quaternion 301 | 0x07, 0x7E, 0x01, 0x30, // CFG_16 inv_set_footer 302 | 0x07, 0x46, 0x01, 0x9A, // CFG_GYRO_SOURCE inv_send_gyro 303 | 0x07, 0x47, 0x04, 0xF1, 0x28, 0x30, 0x38, // CFG_9 inv_send_gyro -> inv_construct3_fifo 304 | 0x07, 0x6C, 0x04, 0xF1, 0x28, 0x30, 0x38, // CFG_12 inv_send_accel -> inv_construct3_fifo 305 | 0x02, 0x16, 0x02, 0x00, 0x01 // D_0_22 inv_set_fifo_rate 306 | 307 | // This very last 0x01 WAS a 0x09, which drops the FIFO rate down to 20 Hz. 0x07 is 25 Hz, 308 | // 0x01 is 100Hz. Going faster than 100Hz (0x00=200Hz) tends to result in very noisy data. 309 | // DMP output frequency is calculated easily using this equation: (200Hz / (1 + value)) 310 | 311 | // It is important to make sure the host processor can keep up with reading and processing 312 | // the FIFO output at the desired rate. Handling FIFO overflow cleanly is also a good idea. 313 | }; 314 | 315 | const unsigned char dmpUpdates[MPU6050_DMP_UPDATES_SIZE] PROGMEM = { 316 | 0x01, 0xB2, 0x02, 0xFF, 0xFF, 317 | 0x01, 0x90, 0x04, 0x09, 0x23, 0xA1, 0x35, 318 | 0x01, 0x6A, 0x02, 0x06, 0x00, 319 | 0x01, 0x60, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 320 | 0x00, 0x60, 0x04, 0x40, 0x00, 0x00, 0x00, 321 | 0x01, 0x62, 0x02, 0x00, 0x00, 322 | 0x00, 0x60, 0x04, 0x00, 0x40, 0x00, 0x00 323 | }; 324 | 325 | uint8_t MPU6050::dmpInitialize() { 326 | // reset device 327 | DEBUG_PRINTLN(F("\n\nResetting MPU6050...")); 328 | reset(); 329 | delay(30); // wait after reset 330 | 331 | // enable sleep mode and wake cycle 332 | /*Serial.println(F("Enabling sleep mode...")); 333 | setSleepEnabled(true); 334 | Serial.println(F("Enabling wake cycle...")); 335 | setWakeCycleEnabled(true);*/ 336 | 337 | // disable sleep mode 338 | DEBUG_PRINTLN(F("Disabling sleep mode...")); 339 | setSleepEnabled(false); 340 | 341 | // get MPU hardware revision 342 | DEBUG_PRINTLN(F("Selecting user bank 16...")); 343 | setMemoryBank(0x10, true, true); 344 | DEBUG_PRINTLN(F("Selecting memory byte 6...")); 345 | setMemoryStartAddress(0x06); 346 | DEBUG_PRINTLN(F("Checking hardware revision...")); 347 | uint8_t hwRevision = readMemoryByte(); 348 | DEBUG_PRINT(F("Revision @ user[16][6] = ")); 349 | DEBUG_PRINTLNF(hwRevision, HEX); 350 | DEBUG_PRINTLN(F("Resetting memory bank selection to 0...")); 351 | setMemoryBank(0, false, false); 352 | 353 | // check OTP bank valid 354 | DEBUG_PRINTLN(F("Reading OTP bank valid flag...")); 355 | uint8_t otpValid = getOTPBankValid(); 356 | DEBUG_PRINT(F("OTP bank is ")); 357 | DEBUG_PRINTLN(otpValid ? F("valid!") : F("invalid!")); 358 | 359 | // get X/Y/Z gyro offsets 360 | DEBUG_PRINTLN(F("Reading gyro offset TC values...")); 361 | int8_t xgOffsetTC = getXGyroOffsetTC(); 362 | int8_t ygOffsetTC = getYGyroOffsetTC(); 363 | int8_t zgOffsetTC = getZGyroOffsetTC(); 364 | DEBUG_PRINT(F("X gyro offset = ")); 365 | DEBUG_PRINTLN(xgOffset); 366 | DEBUG_PRINT(F("Y gyro offset = ")); 367 | DEBUG_PRINTLN(ygOffset); 368 | DEBUG_PRINT(F("Z gyro offset = ")); 369 | DEBUG_PRINTLN(zgOffset); 370 | 371 | // setup weird slave stuff (?) 372 | DEBUG_PRINTLN(F("Setting slave 0 address to 0x7F...")); 373 | setSlaveAddress(0, 0x7F); 374 | DEBUG_PRINTLN(F("Disabling I2C Master mode...")); 375 | setI2CMasterModeEnabled(false); 376 | DEBUG_PRINTLN(F("Setting slave 0 address to 0x68 (self)...")); 377 | setSlaveAddress(0, 0x68); 378 | DEBUG_PRINTLN(F("Resetting I2C Master control...")); 379 | resetI2CMaster(); 380 | delay(20); 381 | 382 | // load DMP code into memory banks 383 | DEBUG_PRINT(F("Writing DMP code to MPU memory banks (")); 384 | DEBUG_PRINT(MPU6050_DMP_CODE_SIZE); 385 | DEBUG_PRINTLN(F(" bytes)")); 386 | if (writeProgMemoryBlock(dmpMemory, MPU6050_DMP_CODE_SIZE)) { 387 | DEBUG_PRINTLN(F("Success! DMP code written and verified.")); 388 | 389 | // write DMP configuration 390 | DEBUG_PRINT(F("Writing DMP configuration to MPU memory banks (")); 391 | DEBUG_PRINT(MPU6050_DMP_CONFIG_SIZE); 392 | DEBUG_PRINTLN(F(" bytes in config def)")); 393 | if (writeProgDMPConfigurationSet(dmpConfig, MPU6050_DMP_CONFIG_SIZE)) { 394 | DEBUG_PRINTLN(F("Success! DMP configuration written and verified.")); 395 | 396 | DEBUG_PRINTLN(F("Setting clock source to Z Gyro...")); 397 | setClockSource(MPU6050_CLOCK_PLL_ZGYRO); 398 | 399 | DEBUG_PRINTLN(F("Setting DMP and FIFO_OFLOW interrupts enabled...")); 400 | setIntEnabled(0x12); 401 | 402 | DEBUG_PRINTLN(F("Setting sample rate to 200Hz...")); 403 | setRate(4); // 1khz / (1 + 4) = 200 Hz 404 | 405 | DEBUG_PRINTLN(F("Setting external frame sync to TEMP_OUT_L[0]...")); 406 | setExternalFrameSync(MPU6050_EXT_SYNC_TEMP_OUT_L); 407 | 408 | DEBUG_PRINTLN(F("Setting DLPF bandwidth to 42Hz...")); 409 | setDLPFMode(MPU6050_DLPF_BW_42); 410 | 411 | DEBUG_PRINTLN(F("Setting gyro sensitivity to +/- 2000 deg/sec...")); 412 | setFullScaleGyroRange(MPU6050_GYRO_FS_2000); 413 | 414 | DEBUG_PRINTLN(F("Setting DMP configuration bytes (function unknown)...")); 415 | setDMPConfig1(0x03); 416 | setDMPConfig2(0x00); 417 | 418 | DEBUG_PRINTLN(F("Clearing OTP Bank flag...")); 419 | setOTPBankValid(false); 420 | 421 | DEBUG_PRINTLN(F("Setting X/Y/Z gyro offset TCs to previous values...")); 422 | setXGyroOffsetTC(xgOffsetTC); 423 | setYGyroOffsetTC(ygOffsetTC); 424 | setZGyroOffsetTC(zgOffsetTC); 425 | 426 | //DEBUG_PRINTLN(F("Setting X/Y/Z gyro user offsets to zero...")); 427 | //setXGyroOffset(0); 428 | //setYGyroOffset(0); 429 | //setZGyroOffset(0); 430 | 431 | DEBUG_PRINTLN(F("Writing final memory update 1/7 (function unknown)...")); 432 | uint8_t dmpUpdate[16], j; 433 | uint16_t pos = 0; 434 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 435 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 436 | 437 | DEBUG_PRINTLN(F("Writing final memory update 2/7 (function unknown)...")); 438 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 439 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 440 | 441 | DEBUG_PRINTLN(F("Resetting FIFO...")); 442 | resetFIFO(); 443 | 444 | DEBUG_PRINTLN(F("Reading FIFO count...")); 445 | uint16_t fifoCount = getFIFOCount(); 446 | uint8_t fifoBuffer[128]; 447 | 448 | DEBUG_PRINT(F("Current FIFO count=")); 449 | DEBUG_PRINTLN(fifoCount); 450 | getFIFOBytes(fifoBuffer, fifoCount); 451 | 452 | DEBUG_PRINTLN(F("Setting motion detection threshold to 2...")); 453 | setMotionDetectionThreshold(2); 454 | 455 | DEBUG_PRINTLN(F("Setting zero-motion detection threshold to 156...")); 456 | setZeroMotionDetectionThreshold(156); 457 | 458 | DEBUG_PRINTLN(F("Setting motion detection duration to 80...")); 459 | setMotionDetectionDuration(80); 460 | 461 | DEBUG_PRINTLN(F("Setting zero-motion detection duration to 0...")); 462 | setZeroMotionDetectionDuration(0); 463 | 464 | DEBUG_PRINTLN(F("Resetting FIFO...")); 465 | resetFIFO(); 466 | 467 | DEBUG_PRINTLN(F("Enabling FIFO...")); 468 | setFIFOEnabled(true); 469 | 470 | DEBUG_PRINTLN(F("Enabling DMP...")); 471 | setDMPEnabled(true); 472 | 473 | DEBUG_PRINTLN(F("Resetting DMP...")); 474 | resetDMP(); 475 | 476 | DEBUG_PRINTLN(F("Writing final memory update 3/7 (function unknown)...")); 477 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 478 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 479 | 480 | DEBUG_PRINTLN(F("Writing final memory update 4/7 (function unknown)...")); 481 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 482 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 483 | 484 | DEBUG_PRINTLN(F("Writing final memory update 5/7 (function unknown)...")); 485 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 486 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 487 | 488 | DEBUG_PRINTLN(F("Waiting for FIFO count > 2...")); 489 | while ((fifoCount = getFIFOCount()) < 3); 490 | 491 | DEBUG_PRINT(F("Current FIFO count=")); 492 | DEBUG_PRINTLN(fifoCount); 493 | DEBUG_PRINTLN(F("Reading FIFO data...")); 494 | getFIFOBytes(fifoBuffer, fifoCount); 495 | 496 | DEBUG_PRINTLN(F("Reading interrupt status...")); 497 | uint8_t mpuIntStatus = getIntStatus(); 498 | 499 | DEBUG_PRINT(F("Current interrupt status=")); 500 | DEBUG_PRINTLNF(mpuIntStatus, HEX); 501 | 502 | DEBUG_PRINTLN(F("Reading final memory update 6/7 (function unknown)...")); 503 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 504 | readMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 505 | 506 | DEBUG_PRINTLN(F("Waiting for FIFO count > 2...")); 507 | while ((fifoCount = getFIFOCount()) < 3); 508 | 509 | DEBUG_PRINT(F("Current FIFO count=")); 510 | DEBUG_PRINTLN(fifoCount); 511 | 512 | DEBUG_PRINTLN(F("Reading FIFO data...")); 513 | getFIFOBytes(fifoBuffer, fifoCount); 514 | 515 | DEBUG_PRINTLN(F("Reading interrupt status...")); 516 | mpuIntStatus = getIntStatus(); 517 | 518 | DEBUG_PRINT(F("Current interrupt status=")); 519 | DEBUG_PRINTLNF(mpuIntStatus, HEX); 520 | 521 | DEBUG_PRINTLN(F("Writing final memory update 7/7 (function unknown)...")); 522 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 523 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 524 | 525 | DEBUG_PRINTLN(F("DMP is good to go! Finally.")); 526 | 527 | DEBUG_PRINTLN(F("Disabling DMP (you turn it on later)...")); 528 | setDMPEnabled(false); 529 | 530 | DEBUG_PRINTLN(F("Setting up internal 42-byte (default) DMP packet buffer...")); 531 | dmpPacketSize = 42; 532 | /*if ((dmpPacketBuffer = (uint8_t *)malloc(42)) == 0) { 533 | return 3; // TODO: proper error code for no memory 534 | }*/ 535 | 536 | DEBUG_PRINTLN(F("Resetting FIFO and clearing INT status one last time...")); 537 | resetFIFO(); 538 | getIntStatus(); 539 | } else { 540 | DEBUG_PRINTLN(F("ERROR! DMP configuration verification failed.")); 541 | return 2; // configuration block loading failed 542 | } 543 | } else { 544 | DEBUG_PRINTLN(F("ERROR! DMP code verification failed.")); 545 | return 1; // main binary block loading failed 546 | } 547 | return 0; // success 548 | } 549 | 550 | bool MPU6050::dmpPacketAvailable() { 551 | return getFIFOCount() >= dmpGetFIFOPacketSize(); 552 | } 553 | 554 | // uint8_t MPU6050::dmpSetFIFORate(uint8_t fifoRate); 555 | // uint8_t MPU6050::dmpGetFIFORate(); 556 | // uint8_t MPU6050::dmpGetSampleStepSizeMS(); 557 | // uint8_t MPU6050::dmpGetSampleFrequency(); 558 | // int32_t MPU6050::dmpDecodeTemperature(int8_t tempReg); 559 | 560 | //uint8_t MPU6050::dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority); 561 | //uint8_t MPU6050::dmpUnregisterFIFORateProcess(inv_obj_func func); 562 | //uint8_t MPU6050::dmpRunFIFORateProcesses(); 563 | 564 | // uint8_t MPU6050::dmpSendQuaternion(uint_fast16_t accuracy); 565 | // uint8_t MPU6050::dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy); 566 | // uint8_t MPU6050::dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy); 567 | // uint8_t MPU6050::dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy); 568 | // uint8_t MPU6050::dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy); 569 | // uint8_t MPU6050::dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy); 570 | // uint8_t MPU6050::dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy); 571 | // uint8_t MPU6050::dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy); 572 | // uint8_t MPU6050::dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy); 573 | // uint8_t MPU6050::dmpSendPacketNumber(uint_fast16_t accuracy); 574 | // uint8_t MPU6050::dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy); 575 | // uint8_t MPU6050::dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy); 576 | 577 | uint8_t MPU6050::dmpGetAccel(int32_t *data, const uint8_t* packet) { 578 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) 579 | if (packet == 0) packet = dmpPacketBuffer; 580 | data[0] = ((packet[28] << 24) + (packet[29] << 16) + (packet[30] << 8) + packet[31]); 581 | data[1] = ((packet[32] << 24) + (packet[33] << 16) + (packet[34] << 8) + packet[35]); 582 | data[2] = ((packet[36] << 24) + (packet[37] << 16) + (packet[38] << 8) + packet[39]); 583 | return 0; 584 | } 585 | uint8_t MPU6050::dmpGetAccel(int16_t *data, const uint8_t* packet) { 586 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) 587 | if (packet == 0) packet = dmpPacketBuffer; 588 | data[0] = (packet[28] << 8) + packet[29]; 589 | data[1] = (packet[32] << 8) + packet[33]; 590 | data[2] = (packet[36] << 8) + packet[37]; 591 | return 0; 592 | } 593 | uint8_t MPU6050::dmpGetAccel(VectorInt16 *v, const uint8_t* packet) { 594 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) 595 | if (packet == 0) packet = dmpPacketBuffer; 596 | v -> x = (packet[28] << 8) + packet[29]; 597 | v -> y = (packet[32] << 8) + packet[33]; 598 | v -> z = (packet[36] << 8) + packet[37]; 599 | return 0; 600 | } 601 | uint8_t MPU6050::dmpGetQuaternion(int32_t *data, const uint8_t* packet) { 602 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) 603 | if (packet == 0) packet = dmpPacketBuffer; 604 | data[0] = ((packet[0] << 24) + (packet[1] << 16) + (packet[2] << 8) + packet[3]); 605 | data[1] = ((packet[4] << 24) + (packet[5] << 16) + (packet[6] << 8) + packet[7]); 606 | data[2] = ((packet[8] << 24) + (packet[9] << 16) + (packet[10] << 8) + packet[11]); 607 | data[3] = ((packet[12] << 24) + (packet[13] << 16) + (packet[14] << 8) + packet[15]); 608 | return 0; 609 | } 610 | uint8_t MPU6050::dmpGetQuaternion(int16_t *data, const uint8_t* packet) { 611 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) 612 | if (packet == 0) packet = dmpPacketBuffer; 613 | data[0] = ((packet[0] << 8) + packet[1]); 614 | data[1] = ((packet[4] << 8) + packet[5]); 615 | data[2] = ((packet[8] << 8) + packet[9]); 616 | data[3] = ((packet[12] << 8) + packet[13]); 617 | return 0; 618 | } 619 | uint8_t MPU6050::dmpGetQuaternion(Quaternion *q, const uint8_t* packet) { 620 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) 621 | int16_t qI[4]; 622 | uint8_t status = dmpGetQuaternion(qI, packet); 623 | if (status == 0) { 624 | q -> w = (float)qI[0] / 16384.0f; 625 | q -> x = (float)qI[1] / 16384.0f; 626 | q -> y = (float)qI[2] / 16384.0f; 627 | q -> z = (float)qI[3] / 16384.0f; 628 | return 0; 629 | } 630 | return status; // int16 return value, indicates error if this line is reached 631 | } 632 | // uint8_t MPU6050::dmpGet6AxisQuaternion(long *data, const uint8_t* packet); 633 | // uint8_t MPU6050::dmpGetRelativeQuaternion(long *data, const uint8_t* packet); 634 | uint8_t MPU6050::dmpGetGyro(int32_t *data, const uint8_t* packet) { 635 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) 636 | if (packet == 0) packet = dmpPacketBuffer; 637 | data[0] = ((packet[16] << 24) + (packet[17] << 16) + (packet[18] << 8) + packet[19]); 638 | data[1] = ((packet[20] << 24) + (packet[21] << 16) + (packet[22] << 8) + packet[23]); 639 | data[2] = ((packet[24] << 24) + (packet[25] << 16) + (packet[26] << 8) + packet[27]); 640 | return 0; 641 | } 642 | uint8_t MPU6050::dmpGetGyro(int16_t *data, const uint8_t* packet) { 643 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) 644 | if (packet == 0) packet = dmpPacketBuffer; 645 | data[0] = (packet[16] << 8) + packet[17]; 646 | data[1] = (packet[20] << 8) + packet[21]; 647 | data[2] = (packet[24] << 8) + packet[25]; 648 | return 0; 649 | } 650 | // uint8_t MPU6050::dmpSetLinearAccelFilterCoefficient(float coef); 651 | // uint8_t MPU6050::dmpGetLinearAccel(long *data, const uint8_t* packet); 652 | uint8_t MPU6050::dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity) { 653 | // get rid of the gravity component (+1g = +8192 in standard DMP FIFO packet, sensitivity is 2g) 654 | v -> x = vRaw -> x - gravity -> x*8192; 655 | v -> y = vRaw -> y - gravity -> y*8192; 656 | v -> z = vRaw -> z - gravity -> z*8192; 657 | return 0; 658 | } 659 | // uint8_t MPU6050::dmpGetLinearAccelInWorld(long *data, const uint8_t* packet); 660 | uint8_t MPU6050::dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q) { 661 | // rotate measured 3D acceleration vector into original state 662 | // frame of reference based on orientation quaternion 663 | memcpy(v, vReal, sizeof(VectorInt16)); 664 | v -> rotate(q); 665 | return 0; 666 | } 667 | // uint8_t MPU6050::dmpGetGyroAndAccelSensor(long *data, const uint8_t* packet); 668 | // uint8_t MPU6050::dmpGetGyroSensor(long *data, const uint8_t* packet); 669 | // uint8_t MPU6050::dmpGetControlData(long *data, const uint8_t* packet); 670 | // uint8_t MPU6050::dmpGetTemperature(long *data, const uint8_t* packet); 671 | // uint8_t MPU6050::dmpGetGravity(long *data, const uint8_t* packet); 672 | uint8_t MPU6050::dmpGetGravity(VectorFloat *v, Quaternion *q) { 673 | v -> x = 2 * (q -> x*q -> z - q -> w*q -> y); 674 | v -> y = 2 * (q -> w*q -> x + q -> y*q -> z); 675 | v -> z = q -> w*q -> w - q -> x*q -> x - q -> y*q -> y + q -> z*q -> z; 676 | return 0; 677 | } 678 | // uint8_t MPU6050::dmpGetUnquantizedAccel(long *data, const uint8_t* packet); 679 | // uint8_t MPU6050::dmpGetQuantizedAccel(long *data, const uint8_t* packet); 680 | // uint8_t MPU6050::dmpGetExternalSensorData(long *data, int size, const uint8_t* packet); 681 | // uint8_t MPU6050::dmpGetEIS(long *data, const uint8_t* packet); 682 | 683 | uint8_t MPU6050::dmpGetEuler(float *data, Quaternion *q) { 684 | data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1); // psi 685 | data[1] = -asin(2*q -> x*q -> z + 2*q -> w*q -> y); // theta 686 | data[2] = atan2(2*q -> y*q -> z - 2*q -> w*q -> x, 2*q -> w*q -> w + 2*q -> z*q -> z - 1); // phi 687 | return 0; 688 | } 689 | uint8_t MPU6050::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) { 690 | // yaw: (about Z axis) 691 | data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1); 692 | // pitch: (nose up/down, about Y axis) 693 | data[1] = atan(gravity -> x / sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z)); 694 | // roll: (tilt left/right, about X axis) 695 | data[2] = atan(gravity -> y / sqrt(gravity -> x*gravity -> x + gravity -> z*gravity -> z)); 696 | return 0; 697 | } 698 | 699 | // uint8_t MPU6050::dmpGetAccelFloat(float *data, const uint8_t* packet); 700 | // uint8_t MPU6050::dmpGetQuaternionFloat(float *data, const uint8_t* packet); 701 | 702 | uint8_t MPU6050::dmpProcessFIFOPacket(const unsigned char *dmpData) { 703 | /*for (uint8_t k = 0; k < dmpPacketSize; k++) { 704 | if (dmpData[k] < 0x10) Serial.print("0"); 705 | Serial.print(dmpData[k], HEX); 706 | Serial.print(" "); 707 | } 708 | Serial.print("\n");*/ 709 | //Serial.println((uint16_t)dmpPacketBuffer); 710 | return 0; 711 | } 712 | uint8_t MPU6050::dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed) { 713 | uint8_t status; 714 | uint8_t buf[dmpPacketSize]; 715 | for (uint8_t i = 0; i < numPackets; i++) { 716 | // read packet from FIFO 717 | getFIFOBytes(buf, dmpPacketSize); 718 | 719 | // process packet 720 | if ((status = dmpProcessFIFOPacket(buf)) > 0) return status; 721 | 722 | // increment external process count variable, if supplied 723 | if (processed != 0) *processed++; 724 | } 725 | return 0; 726 | } 727 | 728 | // uint8_t MPU6050::dmpSetFIFOProcessedCallback(void (*func) (void)); 729 | 730 | // uint8_t MPU6050::dmpInitFIFOParam(); 731 | // uint8_t MPU6050::dmpCloseFIFO(); 732 | // uint8_t MPU6050::dmpSetGyroDataSource(uint_fast8_t source); 733 | // uint8_t MPU6050::dmpDecodeQuantizedAccel(); 734 | // uint32_t MPU6050::dmpGetGyroSumOfSquare(); 735 | // uint32_t MPU6050::dmpGetAccelSumOfSquare(); 736 | // void MPU6050::dmpOverrideQuaternion(long *q); 737 | uint16_t MPU6050::dmpGetFIFOPacketSize() { 738 | return dmpPacketSize; 739 | } 740 | 741 | #endif /* _MPU6050_6AXIS_MOTIONAPPS20_H_ */ 742 | -------------------------------------------------------------------------------- /Ball_Balance_MPU6050_DMP/MPU6050.h: -------------------------------------------------------------------------------- 1 | // I2Cdev library collection - MPU6050 I2C device class 2 | // Based on InvenSense MPU-6050 register map document rev. 2.0, 5/19/2011 (RM-MPU-6000A-00) 3 | // 10/3/2011 by Jeff Rowberg 4 | // Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib 5 | // 6 | // Changelog: 7 | // ... - ongoing debug release 8 | 9 | // NOTE: THIS IS ONLY A PARIAL RELEASE. THIS DEVICE CLASS IS CURRENTLY UNDERGOING ACTIVE 10 | // DEVELOPMENT AND IS STILL MISSING SOME IMPORTANT FEATURES. PLEASE KEEP THIS IN MIND IF 11 | // YOU DECIDE TO USE THIS PARTICULAR CODE FOR ANYTHING. 12 | 13 | /* ============================================ 14 | I2Cdev device library code is placed under the MIT license 15 | Copyright (c) 2012 Jeff Rowberg 16 | 17 | Permission is hereby granted, free of charge, to any person obtaining a copy 18 | of this software and associated documentation files (the "Software"), to deal 19 | in the Software without restriction, including without limitation the rights 20 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 21 | copies of the Software, and to permit persons to whom the Software is 22 | furnished to do so, subject to the following conditions: 23 | 24 | The above copyright notice and this permission notice shall be included in 25 | all copies or substantial portions of the Software. 26 | 27 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 28 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 29 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 30 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 31 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 32 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 33 | THE SOFTWARE. 34 | =============================================== 35 | */ 36 | 37 | #ifndef _MPU6050_H_ 38 | #define _MPU6050_H_ 39 | 40 | #include "I2Cdev.h" 41 | //#include 42 | 43 | 44 | 45 | #define MPU6050_ADDRESS_AD0_LOW 0x68 // address pin low (GND), default for InvenSense evaluation board 46 | #define MPU6050_ADDRESS_AD0_HIGH 0x69 // address pin high (VCC) 47 | #define MPU6050_DEFAULT_ADDRESS MPU6050_ADDRESS_AD0_LOW 48 | 49 | #define MPU6050_RA_XG_OFFS_TC 0x00 //[7] PWR_MODE, [6:1] XG_OFFS_TC, [0] OTP_BNK_VLD 50 | #define MPU6050_RA_YG_OFFS_TC 0x01 //[7] PWR_MODE, [6:1] YG_OFFS_TC, [0] OTP_BNK_VLD 51 | #define MPU6050_RA_ZG_OFFS_TC 0x02 //[7] PWR_MODE, [6:1] ZG_OFFS_TC, [0] OTP_BNK_VLD 52 | #define MPU6050_RA_X_FINE_GAIN 0x03 //[7:0] X_FINE_GAIN 53 | #define MPU6050_RA_Y_FINE_GAIN 0x04 //[7:0] Y_FINE_GAIN 54 | #define MPU6050_RA_Z_FINE_GAIN 0x05 //[7:0] Z_FINE_GAIN 55 | #define MPU6050_RA_XA_OFFS_H 0x06 //[15:0] XA_OFFS 56 | #define MPU6050_RA_XA_OFFS_L_TC 0x07 57 | #define MPU6050_RA_YA_OFFS_H 0x08 //[15:0] YA_OFFS 58 | #define MPU6050_RA_YA_OFFS_L_TC 0x09 59 | #define MPU6050_RA_ZA_OFFS_H 0x0A //[15:0] ZA_OFFS 60 | #define MPU6050_RA_ZA_OFFS_L_TC 0x0B 61 | #define MPU6050_RA_XG_OFFS_USRH 0x13 //[15:0] XG_OFFS_USR 62 | #define MPU6050_RA_XG_OFFS_USRL 0x14 63 | #define MPU6050_RA_YG_OFFS_USRH 0x15 //[15:0] YG_OFFS_USR 64 | #define MPU6050_RA_YG_OFFS_USRL 0x16 65 | #define MPU6050_RA_ZG_OFFS_USRH 0x17 //[15:0] ZG_OFFS_USR 66 | #define MPU6050_RA_ZG_OFFS_USRL 0x18 67 | #define MPU6050_RA_SMPLRT_DIV 0x19 68 | #define MPU6050_RA_CONFIG 0x1A 69 | #define MPU6050_RA_GYRO_CONFIG 0x1B 70 | #define MPU6050_RA_ACCEL_CONFIG 0x1C 71 | #define MPU6050_RA_FF_THR 0x1D 72 | #define MPU6050_RA_FF_DUR 0x1E 73 | #define MPU6050_RA_MOT_THR 0x1F 74 | #define MPU6050_RA_MOT_DUR 0x20 75 | #define MPU6050_RA_ZRMOT_THR 0x21 76 | #define MPU6050_RA_ZRMOT_DUR 0x22 77 | #define MPU6050_RA_FIFO_EN 0x23 78 | #define MPU6050_RA_I2C_MST_CTRL 0x24 79 | #define MPU6050_RA_I2C_SLV0_ADDR 0x25 80 | #define MPU6050_RA_I2C_SLV0_REG 0x26 81 | #define MPU6050_RA_I2C_SLV0_CTRL 0x27 82 | #define MPU6050_RA_I2C_SLV1_ADDR 0x28 83 | #define MPU6050_RA_I2C_SLV1_REG 0x29 84 | #define MPU6050_RA_I2C_SLV1_CTRL 0x2A 85 | #define MPU6050_RA_I2C_SLV2_ADDR 0x2B 86 | #define MPU6050_RA_I2C_SLV2_REG 0x2C 87 | #define MPU6050_RA_I2C_SLV2_CTRL 0x2D 88 | #define MPU6050_RA_I2C_SLV3_ADDR 0x2E 89 | #define MPU6050_RA_I2C_SLV3_REG 0x2F 90 | #define MPU6050_RA_I2C_SLV3_CTRL 0x30 91 | #define MPU6050_RA_I2C_SLV4_ADDR 0x31 92 | #define MPU6050_RA_I2C_SLV4_REG 0x32 93 | #define MPU6050_RA_I2C_SLV4_DO 0x33 94 | #define MPU6050_RA_I2C_SLV4_CTRL 0x34 95 | #define MPU6050_RA_I2C_SLV4_DI 0x35 96 | #define MPU6050_RA_I2C_MST_STATUS 0x36 97 | #define MPU6050_RA_INT_PIN_CFG 0x37 98 | #define MPU6050_RA_INT_ENABLE 0x38 99 | #define MPU6050_RA_DMP_INT_STATUS 0x39 100 | #define MPU6050_RA_INT_STATUS 0x3A 101 | #define MPU6050_RA_ACCEL_XOUT_H 0x3B 102 | #define MPU6050_RA_ACCEL_XOUT_L 0x3C 103 | #define MPU6050_RA_ACCEL_YOUT_H 0x3D 104 | #define MPU6050_RA_ACCEL_YOUT_L 0x3E 105 | #define MPU6050_RA_ACCEL_ZOUT_H 0x3F 106 | #define MPU6050_RA_ACCEL_ZOUT_L 0x40 107 | #define MPU6050_RA_TEMP_OUT_H 0x41 108 | #define MPU6050_RA_TEMP_OUT_L 0x42 109 | #define MPU6050_RA_GYRO_XOUT_H 0x43 110 | #define MPU6050_RA_GYRO_XOUT_L 0x44 111 | #define MPU6050_RA_GYRO_YOUT_H 0x45 112 | #define MPU6050_RA_GYRO_YOUT_L 0x46 113 | #define MPU6050_RA_GYRO_ZOUT_H 0x47 114 | #define MPU6050_RA_GYRO_ZOUT_L 0x48 115 | #define MPU6050_RA_EXT_SENS_DATA_00 0x49 116 | #define MPU6050_RA_EXT_SENS_DATA_01 0x4A 117 | #define MPU6050_RA_EXT_SENS_DATA_02 0x4B 118 | #define MPU6050_RA_EXT_SENS_DATA_03 0x4C 119 | #define MPU6050_RA_EXT_SENS_DATA_04 0x4D 120 | #define MPU6050_RA_EXT_SENS_DATA_05 0x4E 121 | #define MPU6050_RA_EXT_SENS_DATA_06 0x4F 122 | #define MPU6050_RA_EXT_SENS_DATA_07 0x50 123 | #define MPU6050_RA_EXT_SENS_DATA_08 0x51 124 | #define MPU6050_RA_EXT_SENS_DATA_09 0x52 125 | #define MPU6050_RA_EXT_SENS_DATA_10 0x53 126 | #define MPU6050_RA_EXT_SENS_DATA_11 0x54 127 | #define MPU6050_RA_EXT_SENS_DATA_12 0x55 128 | #define MPU6050_RA_EXT_SENS_DATA_13 0x56 129 | #define MPU6050_RA_EXT_SENS_DATA_14 0x57 130 | #define MPU6050_RA_EXT_SENS_DATA_15 0x58 131 | #define MPU6050_RA_EXT_SENS_DATA_16 0x59 132 | #define MPU6050_RA_EXT_SENS_DATA_17 0x5A 133 | #define MPU6050_RA_EXT_SENS_DATA_18 0x5B 134 | #define MPU6050_RA_EXT_SENS_DATA_19 0x5C 135 | #define MPU6050_RA_EXT_SENS_DATA_20 0x5D 136 | #define MPU6050_RA_EXT_SENS_DATA_21 0x5E 137 | #define MPU6050_RA_EXT_SENS_DATA_22 0x5F 138 | #define MPU6050_RA_EXT_SENS_DATA_23 0x60 139 | #define MPU6050_RA_MOT_DETECT_STATUS 0x61 140 | #define MPU6050_RA_I2C_SLV0_DO 0x63 141 | #define MPU6050_RA_I2C_SLV1_DO 0x64 142 | #define MPU6050_RA_I2C_SLV2_DO 0x65 143 | #define MPU6050_RA_I2C_SLV3_DO 0x66 144 | #define MPU6050_RA_I2C_MST_DELAY_CTRL 0x67 145 | #define MPU6050_RA_SIGNAL_PATH_RESET 0x68 146 | #define MPU6050_RA_MOT_DETECT_CTRL 0x69 147 | #define MPU6050_RA_USER_CTRL 0x6A 148 | #define MPU6050_RA_PWR_MGMT_1 0x6B 149 | #define MPU6050_RA_PWR_MGMT_2 0x6C 150 | #define MPU6050_RA_BANK_SEL 0x6D 151 | #define MPU6050_RA_MEM_START_ADDR 0x6E 152 | #define MPU6050_RA_MEM_R_W 0x6F 153 | #define MPU6050_RA_DMP_CFG_1 0x70 154 | #define MPU6050_RA_DMP_CFG_2 0x71 155 | #define MPU6050_RA_FIFO_COUNTH 0x72 156 | #define MPU6050_RA_FIFO_COUNTL 0x73 157 | #define MPU6050_RA_FIFO_R_W 0x74 158 | #define MPU6050_RA_WHO_AM_I 0x75 159 | 160 | #define MPU6050_TC_PWR_MODE_BIT 7 161 | #define MPU6050_TC_OFFSET_BIT 6 162 | #define MPU6050_TC_OFFSET_LENGTH 6 163 | #define MPU6050_TC_OTP_BNK_VLD_BIT 0 164 | 165 | #define MPU6050_VDDIO_LEVEL_VLOGIC 0 166 | #define MPU6050_VDDIO_LEVEL_VDD 1 167 | 168 | #define MPU6050_CFG_EXT_SYNC_SET_BIT 5 169 | #define MPU6050_CFG_EXT_SYNC_SET_LENGTH 3 170 | #define MPU6050_CFG_DLPF_CFG_BIT 2 171 | #define MPU6050_CFG_DLPF_CFG_LENGTH 3 172 | 173 | #define MPU6050_EXT_SYNC_DISABLED 0x0 174 | #define MPU6050_EXT_SYNC_TEMP_OUT_L 0x1 175 | #define MPU6050_EXT_SYNC_GYRO_XOUT_L 0x2 176 | #define MPU6050_EXT_SYNC_GYRO_YOUT_L 0x3 177 | #define MPU6050_EXT_SYNC_GYRO_ZOUT_L 0x4 178 | #define MPU6050_EXT_SYNC_ACCEL_XOUT_L 0x5 179 | #define MPU6050_EXT_SYNC_ACCEL_YOUT_L 0x6 180 | #define MPU6050_EXT_SYNC_ACCEL_ZOUT_L 0x7 181 | 182 | #define MPU6050_DLPF_BW_256 0x00 183 | #define MPU6050_DLPF_BW_188 0x01 184 | #define MPU6050_DLPF_BW_98 0x02 185 | #define MPU6050_DLPF_BW_42 0x03 186 | #define MPU6050_DLPF_BW_20 0x04 187 | #define MPU6050_DLPF_BW_10 0x05 188 | #define MPU6050_DLPF_BW_5 0x06 189 | 190 | #define MPU6050_GCONFIG_FS_SEL_BIT 4 191 | #define MPU6050_GCONFIG_FS_SEL_LENGTH 2 192 | 193 | #define MPU6050_GYRO_FS_250 0x00 194 | #define MPU6050_GYRO_FS_500 0x01 195 | #define MPU6050_GYRO_FS_1000 0x02 196 | #define MPU6050_GYRO_FS_2000 0x03 197 | 198 | #define MPU6050_ACONFIG_XA_ST_BIT 7 199 | #define MPU6050_ACONFIG_YA_ST_BIT 6 200 | #define MPU6050_ACONFIG_ZA_ST_BIT 5 201 | #define MPU6050_ACONFIG_AFS_SEL_BIT 4 202 | #define MPU6050_ACONFIG_AFS_SEL_LENGTH 2 203 | #define MPU6050_ACONFIG_ACCEL_HPF_BIT 2 204 | #define MPU6050_ACONFIG_ACCEL_HPF_LENGTH 3 205 | 206 | #define MPU6050_ACCEL_FS_2 0x00 207 | #define MPU6050_ACCEL_FS_4 0x01 208 | #define MPU6050_ACCEL_FS_8 0x02 209 | #define MPU6050_ACCEL_FS_16 0x03 210 | 211 | #define MPU6050_DHPF_RESET 0x00 212 | #define MPU6050_DHPF_5 0x01 213 | #define MPU6050_DHPF_2P5 0x02 214 | #define MPU6050_DHPF_1P25 0x03 215 | #define MPU6050_DHPF_0P63 0x04 216 | #define MPU6050_DHPF_HOLD 0x07 217 | 218 | #define MPU6050_TEMP_FIFO_EN_BIT 7 219 | #define MPU6050_XG_FIFO_EN_BIT 6 220 | #define MPU6050_YG_FIFO_EN_BIT 5 221 | #define MPU6050_ZG_FIFO_EN_BIT 4 222 | #define MPU6050_ACCEL_FIFO_EN_BIT 3 223 | #define MPU6050_SLV2_FIFO_EN_BIT 2 224 | #define MPU6050_SLV1_FIFO_EN_BIT 1 225 | #define MPU6050_SLV0_FIFO_EN_BIT 0 226 | 227 | #define MPU6050_MULT_MST_EN_BIT 7 228 | #define MPU6050_WAIT_FOR_ES_BIT 6 229 | #define MPU6050_SLV_3_FIFO_EN_BIT 5 230 | #define MPU6050_I2C_MST_P_NSR_BIT 4 231 | #define MPU6050_I2C_MST_CLK_BIT 3 232 | #define MPU6050_I2C_MST_CLK_LENGTH 4 233 | 234 | #define MPU6050_CLOCK_DIV_348 0x0 235 | #define MPU6050_CLOCK_DIV_333 0x1 236 | #define MPU6050_CLOCK_DIV_320 0x2 237 | #define MPU6050_CLOCK_DIV_308 0x3 238 | #define MPU6050_CLOCK_DIV_296 0x4 239 | #define MPU6050_CLOCK_DIV_286 0x5 240 | #define MPU6050_CLOCK_DIV_276 0x6 241 | #define MPU6050_CLOCK_DIV_267 0x7 242 | #define MPU6050_CLOCK_DIV_258 0x8 243 | #define MPU6050_CLOCK_DIV_500 0x9 244 | #define MPU6050_CLOCK_DIV_471 0xA 245 | #define MPU6050_CLOCK_DIV_444 0xB 246 | #define MPU6050_CLOCK_DIV_421 0xC 247 | #define MPU6050_CLOCK_DIV_400 0xD 248 | #define MPU6050_CLOCK_DIV_381 0xE 249 | #define MPU6050_CLOCK_DIV_364 0xF 250 | 251 | #define MPU6050_I2C_SLV_RW_BIT 7 252 | #define MPU6050_I2C_SLV_ADDR_BIT 6 253 | #define MPU6050_I2C_SLV_ADDR_LENGTH 7 254 | #define MPU6050_I2C_SLV_EN_BIT 7 255 | #define MPU6050_I2C_SLV_BYTE_SW_BIT 6 256 | #define MPU6050_I2C_SLV_REG_DIS_BIT 5 257 | #define MPU6050_I2C_SLV_GRP_BIT 4 258 | #define MPU6050_I2C_SLV_LEN_BIT 3 259 | #define MPU6050_I2C_SLV_LEN_LENGTH 4 260 | 261 | #define MPU6050_I2C_SLV4_RW_BIT 7 262 | #define MPU6050_I2C_SLV4_ADDR_BIT 6 263 | #define MPU6050_I2C_SLV4_ADDR_LENGTH 7 264 | #define MPU6050_I2C_SLV4_EN_BIT 7 265 | #define MPU6050_I2C_SLV4_INT_EN_BIT 6 266 | #define MPU6050_I2C_SLV4_REG_DIS_BIT 5 267 | #define MPU6050_I2C_SLV4_MST_DLY_BIT 4 268 | #define MPU6050_I2C_SLV4_MST_DLY_LENGTH 5 269 | 270 | #define MPU6050_MST_PASS_THROUGH_BIT 7 271 | #define MPU6050_MST_I2C_SLV4_DONE_BIT 6 272 | #define MPU6050_MST_I2C_LOST_ARB_BIT 5 273 | #define MPU6050_MST_I2C_SLV4_NACK_BIT 4 274 | #define MPU6050_MST_I2C_SLV3_NACK_BIT 3 275 | #define MPU6050_MST_I2C_SLV2_NACK_BIT 2 276 | #define MPU6050_MST_I2C_SLV1_NACK_BIT 1 277 | #define MPU6050_MST_I2C_SLV0_NACK_BIT 0 278 | 279 | #define MPU6050_INTCFG_INT_LEVEL_BIT 7 280 | #define MPU6050_INTCFG_INT_OPEN_BIT 6 281 | #define MPU6050_INTCFG_LATCH_INT_EN_BIT 5 282 | #define MPU6050_INTCFG_INT_RD_CLEAR_BIT 4 283 | #define MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT 3 284 | #define MPU6050_INTCFG_FSYNC_INT_EN_BIT 2 285 | #define MPU6050_INTCFG_I2C_BYPASS_EN_BIT 1 286 | #define MPU6050_INTCFG_CLKOUT_EN_BIT 0 287 | 288 | #define MPU6050_INTMODE_ACTIVEHIGH 0x00 289 | #define MPU6050_INTMODE_ACTIVELOW 0x01 290 | 291 | #define MPU6050_INTDRV_PUSHPULL 0x00 292 | #define MPU6050_INTDRV_OPENDRAIN 0x01 293 | 294 | #define MPU6050_INTLATCH_50USPULSE 0x00 295 | #define MPU6050_INTLATCH_WAITCLEAR 0x01 296 | 297 | #define MPU6050_INTCLEAR_STATUSREAD 0x00 298 | #define MPU6050_INTCLEAR_ANYREAD 0x01 299 | 300 | #define MPU6050_INTERRUPT_FF_BIT 7 301 | #define MPU6050_INTERRUPT_MOT_BIT 6 302 | #define MPU6050_INTERRUPT_ZMOT_BIT 5 303 | #define MPU6050_INTERRUPT_FIFO_OFLOW_BIT 4 304 | #define MPU6050_INTERRUPT_I2C_MST_INT_BIT 3 305 | #define MPU6050_INTERRUPT_PLL_RDY_INT_BIT 2 306 | #define MPU6050_INTERRUPT_DMP_INT_BIT 1 307 | #define MPU6050_INTERRUPT_DATA_RDY_BIT 0 308 | 309 | // TODO: figure out what these actually do 310 | // UMPL source code is not very obivous 311 | #define MPU6050_DMPINT_5_BIT 5 312 | #define MPU6050_DMPINT_4_BIT 4 313 | #define MPU6050_DMPINT_3_BIT 3 314 | #define MPU6050_DMPINT_2_BIT 2 315 | #define MPU6050_DMPINT_1_BIT 1 316 | #define MPU6050_DMPINT_0_BIT 0 317 | 318 | #define MPU6050_MOTION_MOT_XNEG_BIT 7 319 | #define MPU6050_MOTION_MOT_XPOS_BIT 6 320 | #define MPU6050_MOTION_MOT_YNEG_BIT 5 321 | #define MPU6050_MOTION_MOT_YPOS_BIT 4 322 | #define MPU6050_MOTION_MOT_ZNEG_BIT 3 323 | #define MPU6050_MOTION_MOT_ZPOS_BIT 2 324 | #define MPU6050_MOTION_MOT_ZRMOT_BIT 0 325 | 326 | #define MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT 7 327 | #define MPU6050_DELAYCTRL_I2C_SLV4_DLY_EN_BIT 4 328 | #define MPU6050_DELAYCTRL_I2C_SLV3_DLY_EN_BIT 3 329 | #define MPU6050_DELAYCTRL_I2C_SLV2_DLY_EN_BIT 2 330 | #define MPU6050_DELAYCTRL_I2C_SLV1_DLY_EN_BIT 1 331 | #define MPU6050_DELAYCTRL_I2C_SLV0_DLY_EN_BIT 0 332 | 333 | #define MPU6050_PATHRESET_GYRO_RESET_BIT 2 334 | #define MPU6050_PATHRESET_ACCEL_RESET_BIT 1 335 | #define MPU6050_PATHRESET_TEMP_RESET_BIT 0 336 | 337 | #define MPU6050_DETECT_ACCEL_ON_DELAY_BIT 5 338 | #define MPU6050_DETECT_ACCEL_ON_DELAY_LENGTH 2 339 | #define MPU6050_DETECT_FF_COUNT_BIT 3 340 | #define MPU6050_DETECT_FF_COUNT_LENGTH 2 341 | #define MPU6050_DETECT_MOT_COUNT_BIT 1 342 | #define MPU6050_DETECT_MOT_COUNT_LENGTH 2 343 | 344 | #define MPU6050_DETECT_DECREMENT_RESET 0x0 345 | #define MPU6050_DETECT_DECREMENT_1 0x1 346 | #define MPU6050_DETECT_DECREMENT_2 0x2 347 | #define MPU6050_DETECT_DECREMENT_4 0x3 348 | 349 | #define MPU6050_USERCTRL_DMP_EN_BIT 7 350 | #define MPU6050_USERCTRL_FIFO_EN_BIT 6 351 | #define MPU6050_USERCTRL_I2C_MST_EN_BIT 5 352 | #define MPU6050_USERCTRL_I2C_IF_DIS_BIT 4 353 | #define MPU6050_USERCTRL_DMP_RESET_BIT 3 354 | #define MPU6050_USERCTRL_FIFO_RESET_BIT 2 355 | #define MPU6050_USERCTRL_I2C_MST_RESET_BIT 1 356 | #define MPU6050_USERCTRL_SIG_COND_RESET_BIT 0 357 | 358 | #define MPU6050_PWR1_DEVICE_RESET_BIT 7 359 | #define MPU6050_PWR1_SLEEP_BIT 6 360 | #define MPU6050_PWR1_CYCLE_BIT 5 361 | #define MPU6050_PWR1_TEMP_DIS_BIT 3 362 | #define MPU6050_PWR1_CLKSEL_BIT 2 363 | #define MPU6050_PWR1_CLKSEL_LENGTH 3 364 | 365 | #define MPU6050_CLOCK_INTERNAL 0x00 366 | #define MPU6050_CLOCK_PLL_XGYRO 0x01 367 | #define MPU6050_CLOCK_PLL_YGYRO 0x02 368 | #define MPU6050_CLOCK_PLL_ZGYRO 0x03 369 | #define MPU6050_CLOCK_PLL_EXT32K 0x04 370 | #define MPU6050_CLOCK_PLL_EXT19M 0x05 371 | #define MPU6050_CLOCK_KEEP_RESET 0x07 372 | 373 | #define MPU6050_PWR2_LP_WAKE_CTRL_BIT 7 374 | #define MPU6050_PWR2_LP_WAKE_CTRL_LENGTH 2 375 | #define MPU6050_PWR2_STBY_XA_BIT 5 376 | #define MPU6050_PWR2_STBY_YA_BIT 4 377 | #define MPU6050_PWR2_STBY_ZA_BIT 3 378 | #define MPU6050_PWR2_STBY_XG_BIT 2 379 | #define MPU6050_PWR2_STBY_YG_BIT 1 380 | #define MPU6050_PWR2_STBY_ZG_BIT 0 381 | 382 | #define MPU6050_WAKE_FREQ_1P25 0x0 383 | #define MPU6050_WAKE_FREQ_2P5 0x1 384 | #define MPU6050_WAKE_FREQ_5 0x2 385 | #define MPU6050_WAKE_FREQ_10 0x3 386 | 387 | #define MPU6050_BANKSEL_PRFTCH_EN_BIT 6 388 | #define MPU6050_BANKSEL_CFG_USER_BANK_BIT 5 389 | #define MPU6050_BANKSEL_MEM_SEL_BIT 4 390 | #define MPU6050_BANKSEL_MEM_SEL_LENGTH 5 391 | 392 | #define MPU6050_WHO_AM_I_BIT 6 393 | #define MPU6050_WHO_AM_I_LENGTH 6 394 | 395 | #define MPU6050_DMP_MEMORY_BANKS 8 396 | #define MPU6050_DMP_MEMORY_BANK_SIZE 256 397 | #define MPU6050_DMP_MEMORY_CHUNK_SIZE 16 398 | 399 | // note: DMP code memory blocks defined at end of header file 400 | 401 | class MPU6050 { 402 | public: 403 | MPU6050(); 404 | MPU6050(uint8_t address); 405 | 406 | void initialize(); 407 | bool testConnection(); 408 | 409 | // AUX_VDDIO register 410 | uint8_t getAuxVDDIOLevel(); 411 | void setAuxVDDIOLevel(uint8_t level); 412 | 413 | // SMPLRT_DIV register 414 | uint8_t getRate(); 415 | void setRate(uint8_t rate); 416 | 417 | // CONFIG register 418 | uint8_t getExternalFrameSync(); 419 | void setExternalFrameSync(uint8_t sync); 420 | uint8_t getDLPFMode(); 421 | void setDLPFMode(uint8_t bandwidth); 422 | 423 | // GYRO_CONFIG register 424 | uint8_t getFullScaleGyroRange(); 425 | void setFullScaleGyroRange(uint8_t range); 426 | 427 | // ACCEL_CONFIG register 428 | bool getAccelXSelfTest(); 429 | void setAccelXSelfTest(bool enabled); 430 | bool getAccelYSelfTest(); 431 | void setAccelYSelfTest(bool enabled); 432 | bool getAccelZSelfTest(); 433 | void setAccelZSelfTest(bool enabled); 434 | uint8_t getFullScaleAccelRange(); 435 | void setFullScaleAccelRange(uint8_t range); 436 | uint8_t getDHPFMode(); 437 | void setDHPFMode(uint8_t mode); 438 | 439 | // FF_THR register 440 | uint8_t getFreefallDetectionThreshold(); 441 | void setFreefallDetectionThreshold(uint8_t threshold); 442 | 443 | // FF_DUR register 444 | uint8_t getFreefallDetectionDuration(); 445 | void setFreefallDetectionDuration(uint8_t duration); 446 | 447 | // MOT_THR register 448 | uint8_t getMotionDetectionThreshold(); 449 | void setMotionDetectionThreshold(uint8_t threshold); 450 | 451 | // MOT_DUR register 452 | uint8_t getMotionDetectionDuration(); 453 | void setMotionDetectionDuration(uint8_t duration); 454 | 455 | // ZRMOT_THR register 456 | uint8_t getZeroMotionDetectionThreshold(); 457 | void setZeroMotionDetectionThreshold(uint8_t threshold); 458 | 459 | // ZRMOT_DUR register 460 | uint8_t getZeroMotionDetectionDuration(); 461 | void setZeroMotionDetectionDuration(uint8_t duration); 462 | 463 | // FIFO_EN register 464 | bool getTempFIFOEnabled(); 465 | void setTempFIFOEnabled(bool enabled); 466 | bool getXGyroFIFOEnabled(); 467 | void setXGyroFIFOEnabled(bool enabled); 468 | bool getYGyroFIFOEnabled(); 469 | void setYGyroFIFOEnabled(bool enabled); 470 | bool getZGyroFIFOEnabled(); 471 | void setZGyroFIFOEnabled(bool enabled); 472 | bool getAccelFIFOEnabled(); 473 | void setAccelFIFOEnabled(bool enabled); 474 | bool getSlave2FIFOEnabled(); 475 | void setSlave2FIFOEnabled(bool enabled); 476 | bool getSlave1FIFOEnabled(); 477 | void setSlave1FIFOEnabled(bool enabled); 478 | bool getSlave0FIFOEnabled(); 479 | void setSlave0FIFOEnabled(bool enabled); 480 | 481 | // I2C_MST_CTRL register 482 | bool getMultiMasterEnabled(); 483 | void setMultiMasterEnabled(bool enabled); 484 | bool getWaitForExternalSensorEnabled(); 485 | void setWaitForExternalSensorEnabled(bool enabled); 486 | bool getSlave3FIFOEnabled(); 487 | void setSlave3FIFOEnabled(bool enabled); 488 | bool getSlaveReadWriteTransitionEnabled(); 489 | void setSlaveReadWriteTransitionEnabled(bool enabled); 490 | uint8_t getMasterClockSpeed(); 491 | void setMasterClockSpeed(uint8_t speed); 492 | 493 | // I2C_SLV* registers (Slave 0-3) 494 | uint8_t getSlaveAddress(uint8_t num); 495 | void setSlaveAddress(uint8_t num, uint8_t address); 496 | uint8_t getSlaveRegister(uint8_t num); 497 | void setSlaveRegister(uint8_t num, uint8_t reg); 498 | bool getSlaveEnabled(uint8_t num); 499 | void setSlaveEnabled(uint8_t num, bool enabled); 500 | bool getSlaveWordByteSwap(uint8_t num); 501 | void setSlaveWordByteSwap(uint8_t num, bool enabled); 502 | bool getSlaveWriteMode(uint8_t num); 503 | void setSlaveWriteMode(uint8_t num, bool mode); 504 | bool getSlaveWordGroupOffset(uint8_t num); 505 | void setSlaveWordGroupOffset(uint8_t num, bool enabled); 506 | uint8_t getSlaveDataLength(uint8_t num); 507 | void setSlaveDataLength(uint8_t num, uint8_t length); 508 | 509 | // I2C_SLV* registers (Slave 4) 510 | uint8_t getSlave4Address(); 511 | void setSlave4Address(uint8_t address); 512 | uint8_t getSlave4Register(); 513 | void setSlave4Register(uint8_t reg); 514 | void setSlave4OutputByte(uint8_t data); 515 | bool getSlave4Enabled(); 516 | void setSlave4Enabled(bool enabled); 517 | bool getSlave4InterruptEnabled(); 518 | void setSlave4InterruptEnabled(bool enabled); 519 | bool getSlave4WriteMode(); 520 | void setSlave4WriteMode(bool mode); 521 | uint8_t getSlave4MasterDelay(); 522 | void setSlave4MasterDelay(uint8_t delay); 523 | uint8_t getSlate4InputByte(); 524 | 525 | // I2C_MST_STATUS register 526 | bool getPassthroughStatus(); 527 | bool getSlave4IsDone(); 528 | bool getLostArbitration(); 529 | bool getSlave4Nack(); 530 | bool getSlave3Nack(); 531 | bool getSlave2Nack(); 532 | bool getSlave1Nack(); 533 | bool getSlave0Nack(); 534 | 535 | // INT_PIN_CFG register 536 | bool getInterruptMode(); 537 | void setInterruptMode(bool mode); 538 | bool getInterruptDrive(); 539 | void setInterruptDrive(bool drive); 540 | bool getInterruptLatch(); 541 | void setInterruptLatch(bool latch); 542 | bool getInterruptLatchClear(); 543 | void setInterruptLatchClear(bool clear); 544 | bool getFSyncInterruptLevel(); 545 | void setFSyncInterruptLevel(bool level); 546 | bool getFSyncInterruptEnabled(); 547 | void setFSyncInterruptEnabled(bool enabled); 548 | bool getI2CBypassEnabled(); 549 | void setI2CBypassEnabled(bool enabled); 550 | bool getClockOutputEnabled(); 551 | void setClockOutputEnabled(bool enabled); 552 | 553 | // INT_ENABLE register 554 | uint8_t getIntEnabled(); 555 | void setIntEnabled(uint8_t enabled); 556 | bool getIntFreefallEnabled(); 557 | void setIntFreefallEnabled(bool enabled); 558 | bool getIntMotionEnabled(); 559 | void setIntMotionEnabled(bool enabled); 560 | bool getIntZeroMotionEnabled(); 561 | void setIntZeroMotionEnabled(bool enabled); 562 | bool getIntFIFOBufferOverflowEnabled(); 563 | void setIntFIFOBufferOverflowEnabled(bool enabled); 564 | bool getIntI2CMasterEnabled(); 565 | void setIntI2CMasterEnabled(bool enabled); 566 | bool getIntDataReadyEnabled(); 567 | void setIntDataReadyEnabled(bool enabled); 568 | 569 | // INT_STATUS register 570 | uint8_t getIntStatus(); 571 | bool getIntFreefallStatus(); 572 | bool getIntMotionStatus(); 573 | bool getIntZeroMotionStatus(); 574 | bool getIntFIFOBufferOverflowStatus(); 575 | bool getIntI2CMasterStatus(); 576 | bool getIntDataReadyStatus(); 577 | 578 | // ACCEL_*OUT_* registers 579 | void getMotion9(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz, int16_t* mx, int16_t* my, int16_t* mz); 580 | void getMotion6(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz); 581 | void getAcceleration(int16_t* x, int16_t* y, int16_t* z); 582 | int16_t getAccelerationX(); 583 | int16_t getAccelerationY(); 584 | int16_t getAccelerationZ(); 585 | 586 | // TEMP_OUT_* registers 587 | int16_t getTemperature(); 588 | 589 | // GYRO_*OUT_* registers 590 | void getRotation(int16_t* x, int16_t* y, int16_t* z); 591 | int16_t getRotationX(); 592 | int16_t getRotationY(); 593 | int16_t getRotationZ(); 594 | 595 | // EXT_SENS_DATA_* registers 596 | uint8_t getExternalSensorByte(int position); 597 | uint16_t getExternalSensorWord(int position); 598 | uint32_t getExternalSensorDWord(int position); 599 | 600 | // MOT_DETECT_STATUS register 601 | bool getXNegMotionDetected(); 602 | bool getXPosMotionDetected(); 603 | bool getYNegMotionDetected(); 604 | bool getYPosMotionDetected(); 605 | bool getZNegMotionDetected(); 606 | bool getZPosMotionDetected(); 607 | bool getZeroMotionDetected(); 608 | 609 | // I2C_SLV*_DO register 610 | void setSlaveOutputByte(uint8_t num, uint8_t data); 611 | 612 | // I2C_MST_DELAY_CTRL register 613 | bool getExternalShadowDelayEnabled(); 614 | void setExternalShadowDelayEnabled(bool enabled); 615 | bool getSlaveDelayEnabled(uint8_t num); 616 | void setSlaveDelayEnabled(uint8_t num, bool enabled); 617 | 618 | // SIGNAL_PATH_RESET register 619 | void resetGyroscopePath(); 620 | void resetAccelerometerPath(); 621 | void resetTemperaturePath(); 622 | 623 | // MOT_DETECT_CTRL register 624 | uint8_t getAccelerometerPowerOnDelay(); 625 | void setAccelerometerPowerOnDelay(uint8_t delay); 626 | uint8_t getFreefallDetectionCounterDecrement(); 627 | void setFreefallDetectionCounterDecrement(uint8_t decrement); 628 | uint8_t getMotionDetectionCounterDecrement(); 629 | void setMotionDetectionCounterDecrement(uint8_t decrement); 630 | 631 | // USER_CTRL register 632 | bool getFIFOEnabled(); 633 | void setFIFOEnabled(bool enabled); 634 | bool getI2CMasterModeEnabled(); 635 | void setI2CMasterModeEnabled(bool enabled); 636 | void switchSPIEnabled(bool enabled); 637 | void resetFIFO(); 638 | void resetI2CMaster(); 639 | void resetSensors(); 640 | 641 | // PWR_MGMT_1 register 642 | void reset(); 643 | bool getSleepEnabled(); 644 | void setSleepEnabled(bool enabled); 645 | bool getWakeCycleEnabled(); 646 | void setWakeCycleEnabled(bool enabled); 647 | bool getTempSensorEnabled(); 648 | void setTempSensorEnabled(bool enabled); 649 | uint8_t getClockSource(); 650 | void setClockSource(uint8_t source); 651 | 652 | // PWR_MGMT_2 register 653 | uint8_t getWakeFrequency(); 654 | void setWakeFrequency(uint8_t frequency); 655 | bool getStandbyXAccelEnabled(); 656 | void setStandbyXAccelEnabled(bool enabled); 657 | bool getStandbyYAccelEnabled(); 658 | void setStandbyYAccelEnabled(bool enabled); 659 | bool getStandbyZAccelEnabled(); 660 | void setStandbyZAccelEnabled(bool enabled); 661 | bool getStandbyXGyroEnabled(); 662 | void setStandbyXGyroEnabled(bool enabled); 663 | bool getStandbyYGyroEnabled(); 664 | void setStandbyYGyroEnabled(bool enabled); 665 | bool getStandbyZGyroEnabled(); 666 | void setStandbyZGyroEnabled(bool enabled); 667 | 668 | // FIFO_COUNT_* registers 669 | uint16_t getFIFOCount(); 670 | 671 | // FIFO_R_W register 672 | uint8_t getFIFOByte(); 673 | void setFIFOByte(uint8_t data); 674 | void getFIFOBytes(uint8_t *data, uint8_t length); 675 | 676 | // WHO_AM_I register 677 | uint8_t getDeviceID(); 678 | void setDeviceID(uint8_t id); 679 | 680 | // ======== UNDOCUMENTED/DMP REGISTERS/METHODS ======== 681 | 682 | // XG_OFFS_TC register 683 | uint8_t getOTPBankValid(); 684 | void setOTPBankValid(bool enabled); 685 | int8_t getXGyroOffsetTC(); 686 | void setXGyroOffsetTC(int8_t offset); 687 | 688 | // YG_OFFS_TC register 689 | int8_t getYGyroOffsetTC(); 690 | void setYGyroOffsetTC(int8_t offset); 691 | 692 | // ZG_OFFS_TC register 693 | int8_t getZGyroOffsetTC(); 694 | void setZGyroOffsetTC(int8_t offset); 695 | 696 | // X_FINE_GAIN register 697 | int8_t getXFineGain(); 698 | void setXFineGain(int8_t gain); 699 | 700 | // Y_FINE_GAIN register 701 | int8_t getYFineGain(); 702 | void setYFineGain(int8_t gain); 703 | 704 | // Z_FINE_GAIN register 705 | int8_t getZFineGain(); 706 | void setZFineGain(int8_t gain); 707 | 708 | // XA_OFFS_* registers 709 | int16_t getXAccelOffset(); 710 | void setXAccelOffset(int16_t offset); 711 | 712 | // YA_OFFS_* register 713 | int16_t getYAccelOffset(); 714 | void setYAccelOffset(int16_t offset); 715 | 716 | // ZA_OFFS_* register 717 | int16_t getZAccelOffset(); 718 | void setZAccelOffset(int16_t offset); 719 | 720 | // XG_OFFS_USR* registers 721 | int16_t getXGyroOffset(); 722 | void setXGyroOffset(int16_t offset); 723 | 724 | // YG_OFFS_USR* register 725 | int16_t getYGyroOffset(); 726 | void setYGyroOffset(int16_t offset); 727 | 728 | // ZG_OFFS_USR* register 729 | int16_t getZGyroOffset(); 730 | void setZGyroOffset(int16_t offset); 731 | 732 | // INT_ENABLE register (DMP functions) 733 | bool getIntPLLReadyEnabled(); 734 | void setIntPLLReadyEnabled(bool enabled); 735 | bool getIntDMPEnabled(); 736 | void setIntDMPEnabled(bool enabled); 737 | 738 | // DMP_INT_STATUS 739 | bool getDMPInt5Status(); 740 | bool getDMPInt4Status(); 741 | bool getDMPInt3Status(); 742 | bool getDMPInt2Status(); 743 | bool getDMPInt1Status(); 744 | bool getDMPInt0Status(); 745 | 746 | // INT_STATUS register (DMP functions) 747 | bool getIntPLLReadyStatus(); 748 | bool getIntDMPStatus(); 749 | 750 | // USER_CTRL register (DMP functions) 751 | bool getDMPEnabled(); 752 | void setDMPEnabled(bool enabled); 753 | void resetDMP(); 754 | 755 | // BANK_SEL register 756 | void setMemoryBank(uint8_t bank, bool prefetchEnabled=false, bool userBank=false); 757 | 758 | // MEM_START_ADDR register 759 | void setMemoryStartAddress(uint8_t address); 760 | 761 | // MEM_R_W register 762 | uint8_t readMemoryByte(); 763 | void writeMemoryByte(uint8_t data); 764 | void readMemoryBlock(uint8_t *data, uint16_t dataSize, uint8_t bank=0, uint8_t address=0); 765 | bool writeMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank=0, uint8_t address=0, bool verify=true, bool useProgMem=false); 766 | bool writeProgMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank=0, uint8_t address=0, bool verify=true); 767 | 768 | bool writeDMPConfigurationSet(const uint8_t *data, uint16_t dataSize, bool useProgMem=false); 769 | bool writeProgDMPConfigurationSet(const uint8_t *data, uint16_t dataSize); 770 | 771 | // DMP_CFG_1 register 772 | uint8_t getDMPConfig1(); 773 | void setDMPConfig1(uint8_t config); 774 | 775 | // DMP_CFG_2 register 776 | uint8_t getDMPConfig2(); 777 | void setDMPConfig2(uint8_t config); 778 | 779 | // special methods for MotionApps 2.0 implementation 780 | #ifdef MPU6050_INCLUDE_DMP_MOTIONAPPS20 781 | uint8_t *dmpPacketBuffer; 782 | uint16_t dmpPacketSize; 783 | 784 | uint8_t dmpInitialize(); 785 | bool dmpPacketAvailable(); 786 | 787 | uint8_t dmpSetFIFORate(uint8_t fifoRate); 788 | uint8_t dmpGetFIFORate(); 789 | uint8_t dmpGetSampleStepSizeMS(); 790 | uint8_t dmpGetSampleFrequency(); 791 | int32_t dmpDecodeTemperature(int8_t tempReg); 792 | 793 | // Register callbacks after a packet of FIFO data is processed 794 | //uint8_t dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority); 795 | //uint8_t dmpUnregisterFIFORateProcess(inv_obj_func func); 796 | uint8_t dmpRunFIFORateProcesses(); 797 | 798 | // Setup FIFO for various output 799 | uint8_t dmpSendQuaternion(uint_fast16_t accuracy); 800 | uint8_t dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy); 801 | uint8_t dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy); 802 | uint8_t dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy); 803 | uint8_t dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy); 804 | uint8_t dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy); 805 | uint8_t dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy); 806 | uint8_t dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy); 807 | uint8_t dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy); 808 | uint8_t dmpSendPacketNumber(uint_fast16_t accuracy); 809 | uint8_t dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy); 810 | uint8_t dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy); 811 | 812 | // Get Fixed Point data from FIFO 813 | uint8_t dmpGetAccel(int32_t *data, const uint8_t* packet=0); 814 | uint8_t dmpGetAccel(int16_t *data, const uint8_t* packet=0); 815 | uint8_t dmpGetAccel(VectorInt16 *v, const uint8_t* packet=0); 816 | uint8_t dmpGetQuaternion(int32_t *data, const uint8_t* packet=0); 817 | uint8_t dmpGetQuaternion(int16_t *data, const uint8_t* packet=0); 818 | uint8_t dmpGetQuaternion(Quaternion *q, const uint8_t* packet=0); 819 | uint8_t dmpGet6AxisQuaternion(int32_t *data, const uint8_t* packet=0); 820 | uint8_t dmpGet6AxisQuaternion(int16_t *data, const uint8_t* packet=0); 821 | uint8_t dmpGet6AxisQuaternion(Quaternion *q, const uint8_t* packet=0); 822 | uint8_t dmpGetRelativeQuaternion(int32_t *data, const uint8_t* packet=0); 823 | uint8_t dmpGetRelativeQuaternion(int16_t *data, const uint8_t* packet=0); 824 | uint8_t dmpGetRelativeQuaternion(Quaternion *data, const uint8_t* packet=0); 825 | uint8_t dmpGetGyro(int32_t *data, const uint8_t* packet=0); 826 | uint8_t dmpGetGyro(int16_t *data, const uint8_t* packet=0); 827 | uint8_t dmpGetGyro(VectorInt16 *v, const uint8_t* packet=0); 828 | uint8_t dmpSetLinearAccelFilterCoefficient(float coef); 829 | uint8_t dmpGetLinearAccel(int32_t *data, const uint8_t* packet=0); 830 | uint8_t dmpGetLinearAccel(int16_t *data, const uint8_t* packet=0); 831 | uint8_t dmpGetLinearAccel(VectorInt16 *v, const uint8_t* packet=0); 832 | uint8_t dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity); 833 | uint8_t dmpGetLinearAccelInWorld(int32_t *data, const uint8_t* packet=0); 834 | uint8_t dmpGetLinearAccelInWorld(int16_t *data, const uint8_t* packet=0); 835 | uint8_t dmpGetLinearAccelInWorld(VectorInt16 *v, const uint8_t* packet=0); 836 | uint8_t dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q); 837 | uint8_t dmpGetGyroAndAccelSensor(int32_t *data, const uint8_t* packet=0); 838 | uint8_t dmpGetGyroAndAccelSensor(int16_t *data, const uint8_t* packet=0); 839 | uint8_t dmpGetGyroAndAccelSensor(VectorInt16 *g, VectorInt16 *a, const uint8_t* packet=0); 840 | uint8_t dmpGetGyroSensor(int32_t *data, const uint8_t* packet=0); 841 | uint8_t dmpGetGyroSensor(int16_t *data, const uint8_t* packet=0); 842 | uint8_t dmpGetGyroSensor(VectorInt16 *v, const uint8_t* packet=0); 843 | uint8_t dmpGetControlData(int32_t *data, const uint8_t* packet=0); 844 | uint8_t dmpGetTemperature(int32_t *data, const uint8_t* packet=0); 845 | uint8_t dmpGetGravity(int32_t *data, const uint8_t* packet=0); 846 | uint8_t dmpGetGravity(int16_t *data, const uint8_t* packet=0); 847 | uint8_t dmpGetGravity(VectorInt16 *v, const uint8_t* packet=0); 848 | uint8_t dmpGetGravity(VectorFloat *v, Quaternion *q); 849 | uint8_t dmpGetUnquantizedAccel(int32_t *data, const uint8_t* packet=0); 850 | uint8_t dmpGetUnquantizedAccel(int16_t *data, const uint8_t* packet=0); 851 | uint8_t dmpGetUnquantizedAccel(VectorInt16 *v, const uint8_t* packet=0); 852 | uint8_t dmpGetQuantizedAccel(int32_t *data, const uint8_t* packet=0); 853 | uint8_t dmpGetQuantizedAccel(int16_t *data, const uint8_t* packet=0); 854 | uint8_t dmpGetQuantizedAccel(VectorInt16 *v, const uint8_t* packet=0); 855 | uint8_t dmpGetExternalSensorData(int32_t *data, uint16_t size, const uint8_t* packet=0); 856 | uint8_t dmpGetEIS(int32_t *data, const uint8_t* packet=0); 857 | 858 | uint8_t dmpGetEuler(float *data, Quaternion *q); 859 | uint8_t dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity); 860 | 861 | // Get Floating Point data from FIFO 862 | uint8_t dmpGetAccelFloat(float *data, const uint8_t* packet=0); 863 | uint8_t dmpGetQuaternionFloat(float *data, const uint8_t* packet=0); 864 | 865 | uint8_t dmpProcessFIFOPacket(const unsigned char *dmpData); 866 | uint8_t dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed=NULL); 867 | 868 | uint8_t dmpSetFIFOProcessedCallback(void (*func) (void)); 869 | 870 | uint8_t dmpInitFIFOParam(); 871 | uint8_t dmpCloseFIFO(); 872 | uint8_t dmpSetGyroDataSource(uint8_t source); 873 | uint8_t dmpDecodeQuantizedAccel(); 874 | uint32_t dmpGetGyroSumOfSquare(); 875 | uint32_t dmpGetAccelSumOfSquare(); 876 | void dmpOverrideQuaternion(long *q); 877 | uint16_t dmpGetFIFOPacketSize(); 878 | #endif 879 | 880 | // special methods for MotionApps 4.1 implementation 881 | #ifdef MPU6050_INCLUDE_DMP_MOTIONAPPS41 882 | uint8_t *dmpPacketBuffer; 883 | uint16_t dmpPacketSize; 884 | 885 | uint8_t dmpInitialize(); 886 | bool dmpPacketAvailable(); 887 | 888 | uint8_t dmpSetFIFORate(uint8_t fifoRate); 889 | uint8_t dmpGetFIFORate(); 890 | uint8_t dmpGetSampleStepSizeMS(); 891 | uint8_t dmpGetSampleFrequency(); 892 | int32_t dmpDecodeTemperature(int8_t tempReg); 893 | 894 | // Register callbacks after a packet of FIFO data is processed 895 | //uint8_t dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority); 896 | //uint8_t dmpUnregisterFIFORateProcess(inv_obj_func func); 897 | uint8_t dmpRunFIFORateProcesses(); 898 | 899 | // Setup FIFO for various output 900 | uint8_t dmpSendQuaternion(uint_fast16_t accuracy); 901 | uint8_t dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy); 902 | uint8_t dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy); 903 | uint8_t dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy); 904 | uint8_t dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy); 905 | uint8_t dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy); 906 | uint8_t dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy); 907 | uint8_t dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy); 908 | uint8_t dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy); 909 | uint8_t dmpSendPacketNumber(uint_fast16_t accuracy); 910 | uint8_t dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy); 911 | uint8_t dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy); 912 | 913 | // Get Fixed Point data from FIFO 914 | uint8_t dmpGetAccel(int32_t *data, const uint8_t* packet=0); 915 | uint8_t dmpGetAccel(int16_t *data, const uint8_t* packet=0); 916 | uint8_t dmpGetAccel(VectorInt16 *v, const uint8_t* packet=0); 917 | uint8_t dmpGetQuaternion(int32_t *data, const uint8_t* packet=0); 918 | uint8_t dmpGetQuaternion(int16_t *data, const uint8_t* packet=0); 919 | uint8_t dmpGetQuaternion(Quaternion *q, const uint8_t* packet=0); 920 | uint8_t dmpGet6AxisQuaternion(int32_t *data, const uint8_t* packet=0); 921 | uint8_t dmpGet6AxisQuaternion(int16_t *data, const uint8_t* packet=0); 922 | uint8_t dmpGet6AxisQuaternion(Quaternion *q, const uint8_t* packet=0); 923 | uint8_t dmpGetRelativeQuaternion(int32_t *data, const uint8_t* packet=0); 924 | uint8_t dmpGetRelativeQuaternion(int16_t *data, const uint8_t* packet=0); 925 | uint8_t dmpGetRelativeQuaternion(Quaternion *data, const uint8_t* packet=0); 926 | uint8_t dmpGetGyro(int32_t *data, const uint8_t* packet=0); 927 | uint8_t dmpGetGyro(int16_t *data, const uint8_t* packet=0); 928 | uint8_t dmpGetGyro(VectorInt16 *v, const uint8_t* packet=0); 929 | uint8_t dmpGetMag(int16_t *data, const uint8_t* packet=0); 930 | uint8_t dmpSetLinearAccelFilterCoefficient(float coef); 931 | uint8_t dmpGetLinearAccel(int32_t *data, const uint8_t* packet=0); 932 | uint8_t dmpGetLinearAccel(int16_t *data, const uint8_t* packet=0); 933 | uint8_t dmpGetLinearAccel(VectorInt16 *v, const uint8_t* packet=0); 934 | uint8_t dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity); 935 | uint8_t dmpGetLinearAccelInWorld(int32_t *data, const uint8_t* packet=0); 936 | uint8_t dmpGetLinearAccelInWorld(int16_t *data, const uint8_t* packet=0); 937 | uint8_t dmpGetLinearAccelInWorld(VectorInt16 *v, const uint8_t* packet=0); 938 | uint8_t dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q); 939 | uint8_t dmpGetGyroAndAccelSensor(int32_t *data, const uint8_t* packet=0); 940 | uint8_t dmpGetGyroAndAccelSensor(int16_t *data, const uint8_t* packet=0); 941 | uint8_t dmpGetGyroAndAccelSensor(VectorInt16 *g, VectorInt16 *a, const uint8_t* packet=0); 942 | uint8_t dmpGetGyroSensor(int32_t *data, const uint8_t* packet=0); 943 | uint8_t dmpGetGyroSensor(int16_t *data, const uint8_t* packet=0); 944 | uint8_t dmpGetGyroSensor(VectorInt16 *v, const uint8_t* packet=0); 945 | uint8_t dmpGetControlData(int32_t *data, const uint8_t* packet=0); 946 | uint8_t dmpGetTemperature(int32_t *data, const uint8_t* packet=0); 947 | uint8_t dmpGetGravity(int32_t *data, const uint8_t* packet=0); 948 | uint8_t dmpGetGravity(int16_t *data, const uint8_t* packet=0); 949 | uint8_t dmpGetGravity(VectorInt16 *v, const uint8_t* packet=0); 950 | uint8_t dmpGetGravity(VectorFloat *v, Quaternion *q); 951 | uint8_t dmpGetUnquantizedAccel(int32_t *data, const uint8_t* packet=0); 952 | uint8_t dmpGetUnquantizedAccel(int16_t *data, const uint8_t* packet=0); 953 | uint8_t dmpGetUnquantizedAccel(VectorInt16 *v, const uint8_t* packet=0); 954 | uint8_t dmpGetQuantizedAccel(int32_t *data, const uint8_t* packet=0); 955 | uint8_t dmpGetQuantizedAccel(int16_t *data, const uint8_t* packet=0); 956 | uint8_t dmpGetQuantizedAccel(VectorInt16 *v, const uint8_t* packet=0); 957 | uint8_t dmpGetExternalSensorData(int32_t *data, uint16_t size, const uint8_t* packet=0); 958 | uint8_t dmpGetEIS(int32_t *data, const uint8_t* packet=0); 959 | 960 | uint8_t dmpGetEuler(float *data, Quaternion *q); 961 | uint8_t dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity); 962 | 963 | // Get Floating Point data from FIFO 964 | uint8_t dmpGetAccelFloat(float *data, const uint8_t* packet=0); 965 | uint8_t dmpGetQuaternionFloat(float *data, const uint8_t* packet=0); 966 | 967 | uint8_t dmpProcessFIFOPacket(const unsigned char *dmpData); 968 | uint8_t dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed=NULL); 969 | 970 | uint8_t dmpSetFIFOProcessedCallback(void (*func) (void)); 971 | 972 | uint8_t dmpInitFIFOParam(); 973 | uint8_t dmpCloseFIFO(); 974 | uint8_t dmpSetGyroDataSource(uint8_t source); 975 | uint8_t dmpDecodeQuantizedAccel(); 976 | uint32_t dmpGetGyroSumOfSquare(); 977 | uint32_t dmpGetAccelSumOfSquare(); 978 | void dmpOverrideQuaternion(long *q); 979 | uint16_t dmpGetFIFOPacketSize(); 980 | #endif 981 | 982 | private: 983 | uint8_t devAddr; 984 | uint8_t buffer[14]; 985 | }; 986 | 987 | #endif /* _MPU6050_H_ */ 988 | -------------------------------------------------------------------------------- /Ball_Balance_MPU6050_DMP/I2Cdev.cpp: -------------------------------------------------------------------------------- 1 | // I2Cdev library collection - Main I2C device class 2 | // Abstracts bit and byte I2C R/W functions into a convenient class 3 | // 6/9/2012 by Jeff Rowberg 4 | // 5 | // Changelog: 6 | // 2013-05-06 - add Francesco Ferrara's Fastwire v0.24 implementation with small modifications 7 | // 2013-05-05 - fix issue with writing bit values to words (Sasquatch/Farzanegan) 8 | // 2012-06-09 - fix major issue with reading > 32 bytes at a time with Arduino Wire 9 | // - add compiler warnings when using outdated or IDE or limited I2Cdev implementation 10 | // 2011-11-01 - fix write*Bits mask calculation (thanks sasquatch @ Arduino forums) 11 | // 2011-10-03 - added automatic Arduino version detection for ease of use 12 | // 2011-10-02 - added Gene Knight's NBWire TwoWire class implementation with small modifications 13 | // 2011-08-31 - added support for Arduino 1.0 Wire library (methods are different from 0.x) 14 | // 2011-08-03 - added optional timeout parameter to read* methods to easily change from default 15 | // 2011-08-02 - added support for 16-bit registers 16 | // - fixed incorrect Doxygen comments on some methods 17 | // - added timeout value for read operations (thanks mem @ Arduino forums) 18 | // 2011-07-30 - changed read/write function structures to return success or byte counts 19 | // - made all methods static for multi-device memory savings 20 | // 2011-07-28 - initial release 21 | 22 | /* ============================================ 23 | I2Cdev device library code is placed under the MIT license 24 | Copyright (c) 2013 Jeff Rowberg 25 | 26 | Permission is hereby granted, free of charge, to any person obtaining a copy 27 | of this software and associated documentation files (the "Software"), to deal 28 | in the Software without restriction, including without limitation the rights 29 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 30 | copies of the Software, and to permit persons to whom the Software is 31 | furnished to do so, subject to the following conditions: 32 | 33 | The above copyright notice and this permission notice shall be included in 34 | all copies or substantial portions of the Software. 35 | 36 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 37 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 38 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 39 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 40 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 41 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 42 | THE SOFTWARE. 43 | =============================================== 44 | */ 45 | 46 | #include "I2Cdev.h" 47 | 48 | #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE 49 | 50 | #ifdef I2CDEV_IMPLEMENTATION_WARNINGS 51 | #if ARDUINO < 100 52 | #warning Using outdated Arduino IDE with Wire library is functionally limiting. 53 | #warning Arduino IDE v1.0.1+ with I2Cdev Fastwire implementation is recommended. 54 | #warning This I2Cdev implementation does not support: 55 | #warning - Repeated starts conditions 56 | #warning - Timeout detection (some Wire requests block forever) 57 | #elif ARDUINO == 100 58 | #warning Using outdated Arduino IDE with Wire library is functionally limiting. 59 | #warning Arduino IDE v1.0.1+ with I2Cdev Fastwire implementation is recommended. 60 | #warning This I2Cdev implementation does not support: 61 | #warning - Repeated starts conditions 62 | #warning - Timeout detection (some Wire requests block forever) 63 | #elif ARDUINO > 100 64 | #warning Using current Arduino IDE with Wire library is functionally limiting. 65 | #warning Arduino IDE v1.0.1+ with I2CDEV_BUILTIN_FASTWIRE implementation is recommended. 66 | #warning This I2Cdev implementation does not support: 67 | #warning - Timeout detection (some Wire requests block forever) 68 | #endif 69 | #endif 70 | 71 | #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE 72 | 73 | //#error The I2CDEV_BUILTIN_FASTWIRE implementation is known to be broken right now. Patience, Iago! 74 | 75 | #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE 76 | 77 | #ifdef I2CDEV_IMPLEMENTATION_WARNINGS 78 | #warning Using I2CDEV_BUILTIN_NBWIRE implementation may adversely affect interrupt detection. 79 | #warning This I2Cdev implementation does not support: 80 | #warning - Repeated starts conditions 81 | #endif 82 | 83 | // NBWire implementation based heavily on code by Gene Knight 84 | // Originally posted on the Arduino forum at http://arduino.cc/forum/index.php/topic,70705.0.html 85 | // Originally offered to the i2cdevlib project at http://arduino.cc/forum/index.php/topic,68210.30.html 86 | TwoWire Wire; 87 | 88 | #endif 89 | 90 | /** Default constructor. 91 | */ 92 | I2Cdev::I2Cdev() { 93 | } 94 | 95 | /** Read a single bit from an 8-bit device register. 96 | * @param devAddr I2C slave device address 97 | * @param regAddr Register regAddr to read from 98 | * @param bitNum Bit position to read (0-7) 99 | * @param data Container for single bit value 100 | * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout) 101 | * @return Status of read operation (true = success) 102 | */ 103 | int8_t I2Cdev::readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data, uint16_t timeout) { 104 | uint8_t b; 105 | uint8_t count = readByte(devAddr, regAddr, &b, timeout); 106 | *data = b & (1 << bitNum); 107 | return count; 108 | } 109 | 110 | /** Read a single bit from a 16-bit device register. 111 | * @param devAddr I2C slave device address 112 | * @param regAddr Register regAddr to read from 113 | * @param bitNum Bit position to read (0-15) 114 | * @param data Container for single bit value 115 | * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout) 116 | * @return Status of read operation (true = success) 117 | */ 118 | int8_t I2Cdev::readBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t *data, uint16_t timeout) { 119 | uint16_t b; 120 | uint8_t count = readWord(devAddr, regAddr, &b, timeout); 121 | *data = b & (1 << bitNum); 122 | return count; 123 | } 124 | 125 | /** Read multiple bits from an 8-bit device register. 126 | * @param devAddr I2C slave device address 127 | * @param regAddr Register regAddr to read from 128 | * @param bitStart First bit position to read (0-7) 129 | * @param length Number of bits to read (not more than 8) 130 | * @param data Container for right-aligned value (i.e. '101' read from any bitStart position will equal 0x05) 131 | * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout) 132 | * @return Status of read operation (true = success) 133 | */ 134 | int8_t I2Cdev::readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data, uint16_t timeout) { 135 | // 01101001 read byte 136 | // 76543210 bit numbers 137 | // xxx args: bitStart=4, length=3 138 | // 010 masked 139 | // -> 010 shifted 140 | uint8_t count, b; 141 | if ((count = readByte(devAddr, regAddr, &b, timeout)) != 0) { 142 | uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1); 143 | b &= mask; 144 | b >>= (bitStart - length + 1); 145 | *data = b; 146 | } 147 | return count; 148 | } 149 | 150 | /** Read multiple bits from a 16-bit device register. 151 | * @param devAddr I2C slave device address 152 | * @param regAddr Register regAddr to read from 153 | * @param bitStart First bit position to read (0-15) 154 | * @param length Number of bits to read (not more than 16) 155 | * @param data Container for right-aligned value (i.e. '101' read from any bitStart position will equal 0x05) 156 | * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout) 157 | * @return Status of read operation (1 = success, 0 = failure, -1 = timeout) 158 | */ 159 | int8_t I2Cdev::readBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t *data, uint16_t timeout) { 160 | // 1101011001101001 read byte 161 | // fedcba9876543210 bit numbers 162 | // xxx args: bitStart=12, length=3 163 | // 010 masked 164 | // -> 010 shifted 165 | uint8_t count; 166 | uint16_t w; 167 | if ((count = readWord(devAddr, regAddr, &w, timeout)) != 0) { 168 | uint16_t mask = ((1 << length) - 1) << (bitStart - length + 1); 169 | w &= mask; 170 | w >>= (bitStart - length + 1); 171 | *data = w; 172 | } 173 | return count; 174 | } 175 | 176 | /** Read single byte from an 8-bit device register. 177 | * @param devAddr I2C slave device address 178 | * @param regAddr Register regAddr to read from 179 | * @param data Container for byte value read from device 180 | * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout) 181 | * @return Status of read operation (true = success) 182 | */ 183 | int8_t I2Cdev::readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data, uint16_t timeout) { 184 | return readBytes(devAddr, regAddr, 1, data, timeout); 185 | } 186 | 187 | /** Read single word from a 16-bit device register. 188 | * @param devAddr I2C slave device address 189 | * @param regAddr Register regAddr to read from 190 | * @param data Container for word value read from device 191 | * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout) 192 | * @return Status of read operation (true = success) 193 | */ 194 | int8_t I2Cdev::readWord(uint8_t devAddr, uint8_t regAddr, uint16_t *data, uint16_t timeout) { 195 | return readWords(devAddr, regAddr, 1, data, timeout); 196 | } 197 | 198 | /** Read multiple bytes from an 8-bit device register. 199 | * @param devAddr I2C slave device address 200 | * @param regAddr First register regAddr to read from 201 | * @param length Number of bytes to read 202 | * @param data Buffer to store read data in 203 | * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout) 204 | * @return Number of bytes read (-1 indicates failure) 205 | */ 206 | int8_t I2Cdev::readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, uint16_t timeout) { 207 | #ifdef I2CDEV_SERIAL_DEBUG 208 | Serial.print("I2C (0x"); 209 | Serial.print(devAddr, HEX); 210 | Serial.print(") reading "); 211 | Serial.print(length, DEC); 212 | Serial.print(" bytes from 0x"); 213 | Serial.print(regAddr, HEX); 214 | Serial.print("..."); 215 | #endif 216 | 217 | int8_t count = 0; 218 | uint32_t t1 = millis(); 219 | 220 | #if (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE) 221 | 222 | #if (ARDUINO < 100) 223 | // Arduino v00xx (before v1.0), Wire library 224 | 225 | // I2C/TWI subsystem uses internal buffer that breaks with large data requests 226 | // so if user requests more than BUFFER_LENGTH bytes, we have to do it in 227 | // smaller chunks instead of all at once 228 | for (uint8_t k = 0; k < length; k += min(length, BUFFER_LENGTH)) { 229 | Wire.beginTransmission(devAddr); 230 | Wire.send(regAddr); 231 | Wire.endTransmission(); 232 | Wire.beginTransmission(devAddr); 233 | Wire.requestFrom(devAddr, (uint8_t)min(length - k, BUFFER_LENGTH)); 234 | 235 | for (; Wire.available() && (timeout == 0 || millis() - t1 < timeout); count++) { 236 | data[count] = Wire.receive(); 237 | #ifdef I2CDEV_SERIAL_DEBUG 238 | Serial.print(data[count], HEX); 239 | if (count + 1 < length) Serial.print(" "); 240 | #endif 241 | } 242 | 243 | Wire.endTransmission(); 244 | } 245 | #elif (ARDUINO == 100) 246 | // Arduino v1.0.0, Wire library 247 | // Adds standardized write() and read() stream methods instead of send() and receive() 248 | 249 | // I2C/TWI subsystem uses internal buffer that breaks with large data requests 250 | // so if user requests more than BUFFER_LENGTH bytes, we have to do it in 251 | // smaller chunks instead of all at once 252 | for (uint8_t k = 0; k < length; k += min(length, BUFFER_LENGTH)) { 253 | Wire.beginTransmission(devAddr); 254 | Wire.write(regAddr); 255 | Wire.endTransmission(); 256 | Wire.beginTransmission(devAddr); 257 | Wire.requestFrom(devAddr, (uint8_t)min(length - k, BUFFER_LENGTH)); 258 | 259 | for (; Wire.available() && (timeout == 0 || millis() - t1 < timeout); count++) { 260 | data[count] = Wire.read(); 261 | #ifdef I2CDEV_SERIAL_DEBUG 262 | Serial.print(data[count], HEX); 263 | if (count + 1 < length) Serial.print(" "); 264 | #endif 265 | } 266 | 267 | Wire.endTransmission(); 268 | } 269 | #elif (ARDUINO > 100) 270 | // Arduino v1.0.1+, Wire library 271 | // Adds official support for repeated start condition, yay! 272 | 273 | // I2C/TWI subsystem uses internal buffer that breaks with large data requests 274 | // so if user requests more than BUFFER_LENGTH bytes, we have to do it in 275 | // smaller chunks instead of all at once 276 | for (uint8_t k = 0; k < length; k += min(length, BUFFER_LENGTH)) { 277 | Wire.beginTransmission(devAddr); 278 | Wire.write(regAddr); 279 | Wire.endTransmission(); 280 | Wire.beginTransmission(devAddr); 281 | Wire.requestFrom(devAddr, (uint8_t)min(length - k, BUFFER_LENGTH)); 282 | 283 | for (; Wire.available() && (timeout == 0 || millis() - t1 < timeout); count++) { 284 | data[count] = Wire.read(); 285 | #ifdef I2CDEV_SERIAL_DEBUG 286 | Serial.print(data[count], HEX); 287 | if (count + 1 < length) Serial.print(" "); 288 | #endif 289 | } 290 | } 291 | #endif 292 | 293 | #elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE) 294 | 295 | // Fastwire library 296 | // no loop required for fastwire 297 | uint8_t status = Fastwire::readBuf(devAddr << 1, regAddr, data, length); 298 | if (status == 0) { 299 | count = length; // success 300 | } else { 301 | count = -1; // error 302 | } 303 | 304 | #endif 305 | 306 | // check for timeout 307 | if (timeout > 0 && millis() - t1 >= timeout && count < length) count = -1; // timeout 308 | 309 | #ifdef I2CDEV_SERIAL_DEBUG 310 | Serial.print(". Done ("); 311 | Serial.print(count, DEC); 312 | Serial.println(" read)."); 313 | #endif 314 | 315 | return count; 316 | } 317 | 318 | /** Read multiple words from a 16-bit device register. 319 | * @param devAddr I2C slave device address 320 | * @param regAddr First register regAddr to read from 321 | * @param length Number of words to read 322 | * @param data Buffer to store read data in 323 | * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout) 324 | * @return Number of words read (-1 indicates failure) 325 | */ 326 | int8_t I2Cdev::readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data, uint16_t timeout) { 327 | #ifdef I2CDEV_SERIAL_DEBUG 328 | Serial.print("I2C (0x"); 329 | Serial.print(devAddr, HEX); 330 | Serial.print(") reading "); 331 | Serial.print(length, DEC); 332 | Serial.print(" words from 0x"); 333 | Serial.print(regAddr, HEX); 334 | Serial.print("..."); 335 | #endif 336 | 337 | int8_t count = 0; 338 | uint32_t t1 = millis(); 339 | 340 | #if (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE) 341 | 342 | #if (ARDUINO < 100) 343 | // Arduino v00xx (before v1.0), Wire library 344 | 345 | // I2C/TWI subsystem uses internal buffer that breaks with large data requests 346 | // so if user requests more than BUFFER_LENGTH bytes, we have to do it in 347 | // smaller chunks instead of all at once 348 | for (uint8_t k = 0; k < length * 2; k += min(length * 2, BUFFER_LENGTH)) { 349 | Wire.beginTransmission(devAddr); 350 | Wire.send(regAddr); 351 | Wire.endTransmission(); 352 | Wire.beginTransmission(devAddr); 353 | Wire.requestFrom(devAddr, (uint8_t)(length * 2)); // length=words, this wants bytes 354 | 355 | bool msb = true; // starts with MSB, then LSB 356 | for (; Wire.available() && count < length && (timeout == 0 || millis() - t1 < timeout);) { 357 | if (msb) { 358 | // first byte is bits 15-8 (MSb=15) 359 | data[count] = Wire.receive() << 8; 360 | } else { 361 | // second byte is bits 7-0 (LSb=0) 362 | data[count] |= Wire.receive(); 363 | #ifdef I2CDEV_SERIAL_DEBUG 364 | Serial.print(data[count], HEX); 365 | if (count + 1 < length) Serial.print(" "); 366 | #endif 367 | count++; 368 | } 369 | msb = !msb; 370 | } 371 | 372 | Wire.endTransmission(); 373 | } 374 | #elif (ARDUINO == 100) 375 | // Arduino v1.0.0, Wire library 376 | // Adds standardized write() and read() stream methods instead of send() and receive() 377 | 378 | // I2C/TWI subsystem uses internal buffer that breaks with large data requests 379 | // so if user requests more than BUFFER_LENGTH bytes, we have to do it in 380 | // smaller chunks instead of all at once 381 | for (uint8_t k = 0; k < length * 2; k += min(length * 2, BUFFER_LENGTH)) { 382 | Wire.beginTransmission(devAddr); 383 | Wire.write(regAddr); 384 | Wire.endTransmission(); 385 | Wire.beginTransmission(devAddr); 386 | Wire.requestFrom(devAddr, (uint8_t)(length * 2)); // length=words, this wants bytes 387 | 388 | bool msb = true; // starts with MSB, then LSB 389 | for (; Wire.available() && count < length && (timeout == 0 || millis() - t1 < timeout);) { 390 | if (msb) { 391 | // first byte is bits 15-8 (MSb=15) 392 | data[count] = Wire.read() << 8; 393 | } else { 394 | // second byte is bits 7-0 (LSb=0) 395 | data[count] |= Wire.read(); 396 | #ifdef I2CDEV_SERIAL_DEBUG 397 | Serial.print(data[count], HEX); 398 | if (count + 1 < length) Serial.print(" "); 399 | #endif 400 | count++; 401 | } 402 | msb = !msb; 403 | } 404 | 405 | Wire.endTransmission(); 406 | } 407 | #elif (ARDUINO > 100) 408 | // Arduino v1.0.1+, Wire library 409 | // Adds official support for repeated start condition, yay! 410 | 411 | // I2C/TWI subsystem uses internal buffer that breaks with large data requests 412 | // so if user requests more than BUFFER_LENGTH bytes, we have to do it in 413 | // smaller chunks instead of all at once 414 | for (uint8_t k = 0; k < length * 2; k += min(length * 2, BUFFER_LENGTH)) { 415 | Wire.beginTransmission(devAddr); 416 | Wire.write(regAddr); 417 | Wire.endTransmission(); 418 | Wire.beginTransmission(devAddr); 419 | Wire.requestFrom(devAddr, (uint8_t)(length * 2)); // length=words, this wants bytes 420 | 421 | bool msb = true; // starts with MSB, then LSB 422 | for (; Wire.available() && count < length && (timeout == 0 || millis() - t1 < timeout);) { 423 | if (msb) { 424 | // first byte is bits 15-8 (MSb=15) 425 | data[count] = Wire.read() << 8; 426 | } else { 427 | // second byte is bits 7-0 (LSb=0) 428 | data[count] |= Wire.read(); 429 | #ifdef I2CDEV_SERIAL_DEBUG 430 | Serial.print(data[count], HEX); 431 | if (count + 1 < length) Serial.print(" "); 432 | #endif 433 | count++; 434 | } 435 | msb = !msb; 436 | } 437 | 438 | Wire.endTransmission(); 439 | } 440 | #endif 441 | 442 | #elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE) 443 | 444 | // Fastwire library 445 | // no loop required for fastwire 446 | uint16_t intermediate[(uint8_t)length]; 447 | uint8_t status = Fastwire::readBuf(devAddr << 1, regAddr, (uint8_t *)intermediate, (uint8_t)(length * 2)); 448 | if (status == 0) { 449 | count = length; // success 450 | for (uint8_t i = 0; i < length; i++) { 451 | data[i] = (intermediate[2*i] << 8) | intermediate[2*i + 1]; 452 | } 453 | } else { 454 | count = -1; // error 455 | } 456 | 457 | #endif 458 | 459 | if (timeout > 0 && millis() - t1 >= timeout && count < length) count = -1; // timeout 460 | 461 | #ifdef I2CDEV_SERIAL_DEBUG 462 | Serial.print(". Done ("); 463 | Serial.print(count, DEC); 464 | Serial.println(" read)."); 465 | #endif 466 | 467 | return count; 468 | } 469 | 470 | /** write a single bit in an 8-bit device register. 471 | * @param devAddr I2C slave device address 472 | * @param regAddr Register regAddr to write to 473 | * @param bitNum Bit position to write (0-7) 474 | * @param value New bit value to write 475 | * @return Status of operation (true = success) 476 | */ 477 | bool I2Cdev::writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data) { 478 | uint8_t b; 479 | readByte(devAddr, regAddr, &b); 480 | b = (data != 0) ? (b | (1 << bitNum)) : (b & ~(1 << bitNum)); 481 | return writeByte(devAddr, regAddr, b); 482 | } 483 | 484 | /** write a single bit in a 16-bit device register. 485 | * @param devAddr I2C slave device address 486 | * @param regAddr Register regAddr to write to 487 | * @param bitNum Bit position to write (0-15) 488 | * @param value New bit value to write 489 | * @return Status of operation (true = success) 490 | */ 491 | bool I2Cdev::writeBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t data) { 492 | uint16_t w; 493 | readWord(devAddr, regAddr, &w); 494 | w = (data != 0) ? (w | (1 << bitNum)) : (w & ~(1 << bitNum)); 495 | return writeWord(devAddr, regAddr, w); 496 | } 497 | 498 | /** Write multiple bits in an 8-bit device register. 499 | * @param devAddr I2C slave device address 500 | * @param regAddr Register regAddr to write to 501 | * @param bitStart First bit position to write (0-7) 502 | * @param length Number of bits to write (not more than 8) 503 | * @param data Right-aligned value to write 504 | * @return Status of operation (true = success) 505 | */ 506 | bool I2Cdev::writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data) { 507 | // 010 value to write 508 | // 76543210 bit numbers 509 | // xxx args: bitStart=4, length=3 510 | // 00011100 mask byte 511 | // 10101111 original value (sample) 512 | // 10100011 original & ~mask 513 | // 10101011 masked | value 514 | uint8_t b; 515 | if (readByte(devAddr, regAddr, &b) != 0) { 516 | uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1); 517 | data <<= (bitStart - length + 1); // shift data into correct position 518 | data &= mask; // zero all non-important bits in data 519 | b &= ~(mask); // zero all important bits in existing byte 520 | b |= data; // combine data with existing byte 521 | return writeByte(devAddr, regAddr, b); 522 | } else { 523 | return false; 524 | } 525 | } 526 | 527 | /** Write multiple bits in a 16-bit device register. 528 | * @param devAddr I2C slave device address 529 | * @param regAddr Register regAddr to write to 530 | * @param bitStart First bit position to write (0-15) 531 | * @param length Number of bits to write (not more than 16) 532 | * @param data Right-aligned value to write 533 | * @return Status of operation (true = success) 534 | */ 535 | bool I2Cdev::writeBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t data) { 536 | // 010 value to write 537 | // fedcba9876543210 bit numbers 538 | // xxx args: bitStart=12, length=3 539 | // 0001110000000000 mask word 540 | // 1010111110010110 original value (sample) 541 | // 1010001110010110 original & ~mask 542 | // 1010101110010110 masked | value 543 | uint16_t w; 544 | if (readWord(devAddr, regAddr, &w) != 0) { 545 | uint16_t mask = ((1 << length) - 1) << (bitStart - length + 1); 546 | data <<= (bitStart - length + 1); // shift data into correct position 547 | data &= mask; // zero all non-important bits in data 548 | w &= ~(mask); // zero all important bits in existing word 549 | w |= data; // combine data with existing word 550 | return writeWord(devAddr, regAddr, w); 551 | } else { 552 | return false; 553 | } 554 | } 555 | 556 | /** Write single byte to an 8-bit device register. 557 | * @param devAddr I2C slave device address 558 | * @param regAddr Register address to write to 559 | * @param data New byte value to write 560 | * @return Status of operation (true = success) 561 | */ 562 | bool I2Cdev::writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data) { 563 | return writeBytes(devAddr, regAddr, 1, &data); 564 | } 565 | 566 | /** Write single word to a 16-bit device register. 567 | * @param devAddr I2C slave device address 568 | * @param regAddr Register address to write to 569 | * @param data New word value to write 570 | * @return Status of operation (true = success) 571 | */ 572 | bool I2Cdev::writeWord(uint8_t devAddr, uint8_t regAddr, uint16_t data) { 573 | return writeWords(devAddr, regAddr, 1, &data); 574 | } 575 | 576 | /** Write multiple bytes to an 8-bit device register. 577 | * @param devAddr I2C slave device address 578 | * @param regAddr First register address to write to 579 | * @param length Number of bytes to write 580 | * @param data Buffer to copy new data from 581 | * @return Status of operation (true = success) 582 | */ 583 | bool I2Cdev::writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t* data) { 584 | #ifdef I2CDEV_SERIAL_DEBUG 585 | Serial.print("I2C (0x"); 586 | Serial.print(devAddr, HEX); 587 | Serial.print(") writing "); 588 | Serial.print(length, DEC); 589 | Serial.print(" bytes to 0x"); 590 | Serial.print(regAddr, HEX); 591 | Serial.print("..."); 592 | #endif 593 | uint8_t status = 0; 594 | #if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE) 595 | Wire.beginTransmission(devAddr); 596 | Wire.send((uint8_t) regAddr); // send address 597 | #elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100) 598 | Wire.beginTransmission(devAddr); 599 | Wire.write((uint8_t) regAddr); // send address 600 | #elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE) 601 | Fastwire::beginTransmission(devAddr); 602 | Fastwire::write(regAddr); 603 | #endif 604 | for (uint8_t i = 0; i < length; i++) { 605 | #ifdef I2CDEV_SERIAL_DEBUG 606 | Serial.print(data[i], HEX); 607 | if (i + 1 < length) Serial.print(" "); 608 | #endif 609 | #if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE) 610 | Wire.send((uint8_t) data[i]); 611 | #elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100) 612 | Wire.write((uint8_t) data[i]); 613 | #elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE) 614 | Fastwire::write((uint8_t) data[i]); 615 | #endif 616 | } 617 | #if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE) 618 | Wire.endTransmission(); 619 | #elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100) 620 | status = Wire.endTransmission(); 621 | #elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE) 622 | Fastwire::stop(); 623 | //status = Fastwire::endTransmission(); 624 | #endif 625 | #ifdef I2CDEV_SERIAL_DEBUG 626 | Serial.println(". Done."); 627 | #endif 628 | return status == 0; 629 | } 630 | 631 | /** Write multiple words to a 16-bit device register. 632 | * @param devAddr I2C slave device address 633 | * @param regAddr First register address to write to 634 | * @param length Number of words to write 635 | * @param data Buffer to copy new data from 636 | * @return Status of operation (true = success) 637 | */ 638 | bool I2Cdev::writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t* data) { 639 | #ifdef I2CDEV_SERIAL_DEBUG 640 | Serial.print("I2C (0x"); 641 | Serial.print(devAddr, HEX); 642 | Serial.print(") writing "); 643 | Serial.print(length, DEC); 644 | Serial.print(" words to 0x"); 645 | Serial.print(regAddr, HEX); 646 | Serial.print("..."); 647 | #endif 648 | uint8_t status = 0; 649 | #if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE) 650 | Wire.beginTransmission(devAddr); 651 | Wire.send(regAddr); // send address 652 | #elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100) 653 | Wire.beginTransmission(devAddr); 654 | Wire.write(regAddr); // send address 655 | #elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE) 656 | Fastwire::beginTransmission(devAddr); 657 | Fastwire::write(regAddr); 658 | #endif 659 | for (uint8_t i = 0; i < length * 2; i++) { 660 | #ifdef I2CDEV_SERIAL_DEBUG 661 | Serial.print(data[i], HEX); 662 | if (i + 1 < length) Serial.print(" "); 663 | #endif 664 | #if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE) 665 | Wire.send((uint8_t)(data[i] >> 8)); // send MSB 666 | Wire.send((uint8_t)data[i++]); // send LSB 667 | #elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100) 668 | Wire.write((uint8_t)(data[i] >> 8)); // send MSB 669 | Wire.write((uint8_t)data[i++]); // send LSB 670 | #elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE) 671 | Fastwire::write((uint8_t)(data[i] >> 8)); // send MSB 672 | status = Fastwire::write((uint8_t)data[i++]); // send LSB 673 | if (status != 0) break; 674 | #endif 675 | } 676 | #if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE) 677 | Wire.endTransmission(); 678 | #elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100) 679 | status = Wire.endTransmission(); 680 | #elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE) 681 | Fastwire::stop(); 682 | //status = Fastwire::endTransmission(); 683 | #endif 684 | #ifdef I2CDEV_SERIAL_DEBUG 685 | Serial.println(". Done."); 686 | #endif 687 | return status == 0; 688 | } 689 | 690 | /** Default timeout value for read operations. 691 | * Set this to 0 to disable timeout detection. 692 | */ 693 | uint16_t I2Cdev::readTimeout = I2CDEV_DEFAULT_READ_TIMEOUT; 694 | 695 | #if I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE 696 | // I2C library 697 | ////////////////////// 698 | // Copyright(C) 2012 699 | // Francesco Ferrara 700 | // ferrara[at]libero[point]it 701 | ////////////////////// 702 | 703 | /* 704 | FastWire 705 | - 0.24 added stop 706 | - 0.23 added reset 707 | 708 | This is a library to help faster programs to read I2C devices. 709 | Copyright(C) 2012 Francesco Ferrara 710 | occhiobello at gmail dot com 711 | [used by Jeff Rowberg for I2Cdevlib with permission] 712 | */ 713 | 714 | boolean Fastwire::waitInt() { 715 | int l = 250; 716 | while (!(TWCR & (1 << TWINT)) && l-- > 0); 717 | return l > 0; 718 | } 719 | 720 | void Fastwire::setup(int khz, boolean pullup) { 721 | TWCR = 0; 722 | #if defined(__AVR_ATmega168__) || defined(__AVR_ATmega8__) || defined(__AVR_ATmega328P__) 723 | // activate internal pull-ups for twi (PORTC bits 4 & 5) 724 | // as per note from atmega8 manual pg167 725 | if (pullup) PORTC |= ((1 << 4) | (1 << 5)); 726 | else PORTC &= ~((1 << 4) | (1 << 5)); 727 | #elif defined(__AVR_ATmega644P__) || defined(__AVR_ATmega644__) 728 | // activate internal pull-ups for twi (PORTC bits 0 & 1) 729 | if (pullup) PORTC |= ((1 << 0) | (1 << 1)); 730 | else PORTC &= ~((1 << 0) | (1 << 1)); 731 | #else 732 | // activate internal pull-ups for twi (PORTD bits 0 & 1) 733 | // as per note from atmega128 manual pg204 734 | if (pullup) PORTD |= ((1 << 0) | (1 << 1)); 735 | else PORTD &= ~((1 << 0) | (1 << 1)); 736 | #endif 737 | 738 | TWSR = 0; // no prescaler => prescaler = 1 739 | TWBR = ((16000L / khz) - 16) / 2; // change the I2C clock rate 740 | TWCR = 1 << TWEN; // enable twi module, no interrupt 741 | } 742 | 743 | // added by Jeff Rowberg 2013-05-07: 744 | // Arduino Wire-style "beginTransmission" function 745 | // (takes 7-bit device address like the Wire method, NOT 8-bit: 0x68, not 0xD0/0xD1) 746 | byte Fastwire::beginTransmission(byte device) { 747 | byte twst, retry; 748 | retry = 2; 749 | do { 750 | TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWSTO) | (1 << TWSTA); 751 | if (!waitInt()) return 1; 752 | twst = TWSR & 0xF8; 753 | if (twst != TW_START && twst != TW_REP_START) return 2; 754 | 755 | //Serial.print(device, HEX); 756 | //Serial.print(" "); 757 | TWDR = device << 1; // send device address without read bit (1) 758 | TWCR = (1 << TWINT) | (1 << TWEN); 759 | if (!waitInt()) return 3; 760 | twst = TWSR & 0xF8; 761 | } while (twst == TW_MT_SLA_NACK && retry-- > 0); 762 | if (twst != TW_MT_SLA_ACK) return 4; 763 | return 0; 764 | } 765 | 766 | byte Fastwire::writeBuf(byte device, byte address, byte *data, byte num) { 767 | byte twst, retry; 768 | 769 | retry = 2; 770 | do { 771 | TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWSTO) | (1 << TWSTA); 772 | if (!waitInt()) return 1; 773 | twst = TWSR & 0xF8; 774 | if (twst != TW_START && twst != TW_REP_START) return 2; 775 | 776 | //Serial.print(device, HEX); 777 | //Serial.print(" "); 778 | TWDR = device & 0xFE; // send device address without read bit (1) 779 | TWCR = (1 << TWINT) | (1 << TWEN); 780 | if (!waitInt()) return 3; 781 | twst = TWSR & 0xF8; 782 | } while (twst == TW_MT_SLA_NACK && retry-- > 0); 783 | if (twst != TW_MT_SLA_ACK) return 4; 784 | 785 | //Serial.print(address, HEX); 786 | //Serial.print(" "); 787 | TWDR = address; // send data to the previously addressed device 788 | TWCR = (1 << TWINT) | (1 << TWEN); 789 | if (!waitInt()) return 5; 790 | twst = TWSR & 0xF8; 791 | if (twst != TW_MT_DATA_ACK) return 6; 792 | 793 | for (byte i = 0; i < num; i++) { 794 | //Serial.print(data[i], HEX); 795 | //Serial.print(" "); 796 | TWDR = data[i]; // send data to the previously addressed device 797 | TWCR = (1 << TWINT) | (1 << TWEN); 798 | if (!waitInt()) return 7; 799 | twst = TWSR & 0xF8; 800 | if (twst != TW_MT_DATA_ACK) return 8; 801 | } 802 | //Serial.print("\n"); 803 | 804 | return 0; 805 | } 806 | 807 | byte Fastwire::write(byte value) { 808 | byte twst; 809 | //Serial.println(value, HEX); 810 | TWDR = value; // send data 811 | TWCR = (1 << TWINT) | (1 << TWEN); 812 | if (!waitInt()) return 1; 813 | twst = TWSR & 0xF8; 814 | if (twst != TW_MT_DATA_ACK) return 2; 815 | return 0; 816 | } 817 | 818 | byte Fastwire::readBuf(byte device, byte address, byte *data, byte num) { 819 | byte twst, retry; 820 | 821 | retry = 2; 822 | do { 823 | TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWSTO) | (1 << TWSTA); 824 | if (!waitInt()) return 16; 825 | twst = TWSR & 0xF8; 826 | if (twst != TW_START && twst != TW_REP_START) return 17; 827 | 828 | //Serial.print(device, HEX); 829 | //Serial.print(" "); 830 | TWDR = device & 0xfe; // send device address to write 831 | TWCR = (1 << TWINT) | (1 << TWEN); 832 | if (!waitInt()) return 18; 833 | twst = TWSR & 0xF8; 834 | } while (twst == TW_MT_SLA_NACK && retry-- > 0); 835 | if (twst != TW_MT_SLA_ACK) return 19; 836 | 837 | //Serial.print(address, HEX); 838 | //Serial.print(" "); 839 | TWDR = address; // send data to the previously addressed device 840 | TWCR = (1 << TWINT) | (1 << TWEN); 841 | if (!waitInt()) return 20; 842 | twst = TWSR & 0xF8; 843 | if (twst != TW_MT_DATA_ACK) return 21; 844 | 845 | /***/ 846 | 847 | retry = 2; 848 | do { 849 | TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWSTO) | (1 << TWSTA); 850 | if (!waitInt()) return 22; 851 | twst = TWSR & 0xF8; 852 | if (twst != TW_START && twst != TW_REP_START) return 23; 853 | 854 | //Serial.print(device, HEX); 855 | //Serial.print(" "); 856 | TWDR = device | 0x01; // send device address with the read bit (1) 857 | TWCR = (1 << TWINT) | (1 << TWEN); 858 | if (!waitInt()) return 24; 859 | twst = TWSR & 0xF8; 860 | } while (twst == TW_MR_SLA_NACK && retry-- > 0); 861 | if (twst != TW_MR_SLA_ACK) return 25; 862 | 863 | for (uint8_t i = 0; i < num; i++) { 864 | if (i == num - 1) 865 | TWCR = (1 << TWINT) | (1 << TWEN); 866 | else 867 | TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWEA); 868 | if (!waitInt()) return 26; 869 | twst = TWSR & 0xF8; 870 | if (twst != TW_MR_DATA_ACK && twst != TW_MR_DATA_NACK) return twst; 871 | data[i] = TWDR; 872 | //Serial.print(data[i], HEX); 873 | //Serial.print(" "); 874 | } 875 | //Serial.print("\n"); 876 | stop(); 877 | 878 | return 0; 879 | } 880 | 881 | void Fastwire::reset() { 882 | TWCR = 0; 883 | } 884 | 885 | byte Fastwire::stop() { 886 | TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWSTO); 887 | if (!waitInt()) return 1; 888 | return 0; 889 | } 890 | #endif 891 | 892 | #if I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE 893 | // NBWire implementation based heavily on code by Gene Knight 894 | // Originally posted on the Arduino forum at http://arduino.cc/forum/index.php/topic,70705.0.html 895 | // Originally offered to the i2cdevlib project at http://arduino.cc/forum/index.php/topic,68210.30.html 896 | 897 | /* 898 | call this version 1.0 899 | 900 | Offhand, the only funky part that I can think of is in nbrequestFrom, where the buffer 901 | length and index are set *before* the data is actually read. The problem is that these 902 | are variables local to the TwoWire object, and by the time we actually have read the 903 | data, and know what the length actually is, we have no simple access to the object's 904 | variables. The actual bytes read *is* given to the callback function, though. 905 | 906 | The ISR code for a slave receiver is commented out. I don't have that setup, and can't 907 | verify it at this time. Save it for 2.0! 908 | 909 | The handling of the read and write processes here is much like in the demo sketch code: 910 | the process is broken down into sequential functions, where each registers the next as a 911 | callback, essentially. 912 | 913 | For example, for the Read process, twi_read00 just returns if TWI is not yet in a 914 | ready state. When there's another interrupt, and the interface *is* ready, then it 915 | sets up the read, starts it, and registers twi_read01 as the function to call after 916 | the *next* interrupt. twi_read01, then, just returns if the interface is still in a 917 | "reading" state. When the reading is done, it copies the information to the buffer, 918 | cleans up, and calls the user-requested callback function with the actual number of 919 | bytes read. 920 | 921 | The writing is similar. 922 | 923 | Questions, comments and problems can go to Gene@Telobot.com. 924 | 925 | Thumbs Up! 926 | Gene Knight 927 | 928 | */ 929 | 930 | uint8_t TwoWire::rxBuffer[NBWIRE_BUFFER_LENGTH]; 931 | uint8_t TwoWire::rxBufferIndex = 0; 932 | uint8_t TwoWire::rxBufferLength = 0; 933 | 934 | uint8_t TwoWire::txAddress = 0; 935 | uint8_t TwoWire::txBuffer[NBWIRE_BUFFER_LENGTH]; 936 | uint8_t TwoWire::txBufferIndex = 0; 937 | uint8_t TwoWire::txBufferLength = 0; 938 | 939 | //uint8_t TwoWire::transmitting = 0; 940 | void (*TwoWire::user_onRequest)(void); 941 | void (*TwoWire::user_onReceive)(int); 942 | 943 | static volatile uint8_t twi_transmitting; 944 | static volatile uint8_t twi_state; 945 | static uint8_t twi_slarw; 946 | static volatile uint8_t twi_error; 947 | static uint8_t twi_masterBuffer[TWI_BUFFER_LENGTH]; 948 | static volatile uint8_t twi_masterBufferIndex; 949 | static uint8_t twi_masterBufferLength; 950 | static uint8_t twi_rxBuffer[TWI_BUFFER_LENGTH]; 951 | static volatile uint8_t twi_rxBufferIndex; 952 | //static volatile uint8_t twi_Interrupt_Continue_Command; 953 | static volatile uint8_t twi_Return_Value; 954 | static volatile uint8_t twi_Done; 955 | void (*twi_cbendTransmissionDone)(int); 956 | void (*twi_cbreadFromDone)(int); 957 | 958 | void twi_init() { 959 | // initialize state 960 | twi_state = TWI_READY; 961 | 962 | // activate internal pull-ups for twi 963 | // as per note from atmega8 manual pg167 964 | sbi(PORTC, 4); 965 | sbi(PORTC, 5); 966 | 967 | // initialize twi prescaler and bit rate 968 | cbi(TWSR, TWPS0); // TWI Status Register - Prescaler bits 969 | cbi(TWSR, TWPS1); 970 | 971 | /* twi bit rate formula from atmega128 manual pg 204 972 | SCL Frequency = CPU Clock Frequency / (16 + (2 * TWBR)) 973 | note: TWBR should be 10 or higher for master mode 974 | It is 72 for a 16mhz Wiring board with 100kHz TWI */ 975 | 976 | TWBR = ((CPU_FREQ / TWI_FREQ) - 16) / 2; // bitrate register 977 | // enable twi module, acks, and twi interrupt 978 | 979 | TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA); 980 | 981 | /* TWEN - TWI Enable Bit 982 | TWIE - TWI Interrupt Enable 983 | TWEA - TWI Enable Acknowledge Bit 984 | TWINT - TWI Interrupt Flag 985 | TWSTA - TWI Start Condition 986 | */ 987 | } 988 | 989 | typedef struct { 990 | uint8_t address; 991 | uint8_t* data; 992 | uint8_t length; 993 | uint8_t wait; 994 | uint8_t i; 995 | } twi_Write_Vars; 996 | 997 | twi_Write_Vars *ptwv = 0; 998 | static void (*fNextInterruptFunction)(void) = 0; 999 | 1000 | void twi_Finish(byte bRetVal) { 1001 | if (ptwv) { 1002 | free(ptwv); 1003 | ptwv = 0; 1004 | } 1005 | twi_Done = 0xFF; 1006 | twi_Return_Value = bRetVal; 1007 | fNextInterruptFunction = 0; 1008 | } 1009 | 1010 | uint8_t twii_WaitForDone(uint16_t timeout) { 1011 | uint32_t endMillis = millis() + timeout; 1012 | while (!twi_Done && (timeout == 0 || millis() < endMillis)) continue; 1013 | return twi_Return_Value; 1014 | } 1015 | 1016 | void twii_SetState(uint8_t ucState) { 1017 | twi_state = ucState; 1018 | } 1019 | 1020 | void twii_SetError(uint8_t ucError) { 1021 | twi_error = ucError ; 1022 | } 1023 | 1024 | void twii_InitBuffer(uint8_t ucPos, uint8_t ucLength) { 1025 | twi_masterBufferIndex = 0; 1026 | twi_masterBufferLength = ucLength; 1027 | } 1028 | 1029 | void twii_CopyToBuf(uint8_t* pData, uint8_t ucLength) { 1030 | uint8_t i; 1031 | for (i = 0; i < ucLength; ++i) { 1032 | twi_masterBuffer[i] = pData[i]; 1033 | } 1034 | } 1035 | 1036 | void twii_CopyFromBuf(uint8_t *pData, uint8_t ucLength) { 1037 | uint8_t i; 1038 | for (i = 0; i < ucLength; ++i) { 1039 | pData[i] = twi_masterBuffer[i]; 1040 | } 1041 | } 1042 | 1043 | void twii_SetSlaRW(uint8_t ucSlaRW) { 1044 | twi_slarw = ucSlaRW; 1045 | } 1046 | 1047 | void twii_SetStart() { 1048 | TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA) | _BV(TWINT) | _BV(TWSTA); 1049 | } 1050 | 1051 | void twi_write01() { 1052 | if (TWI_MTX == twi_state) return; // blocking test 1053 | twi_transmitting = 0 ; 1054 | if (twi_error == 0xFF) 1055 | twi_Finish (0); // success 1056 | else if (twi_error == TW_MT_SLA_NACK) 1057 | twi_Finish (2); // error: address send, nack received 1058 | else if (twi_error == TW_MT_DATA_NACK) 1059 | twi_Finish (3); // error: data send, nack received 1060 | else 1061 | twi_Finish (4); // other twi error 1062 | if (twi_cbendTransmissionDone) return twi_cbendTransmissionDone(twi_Return_Value); 1063 | return; 1064 | } 1065 | 1066 | 1067 | void twi_write00() { 1068 | if (TWI_READY != twi_state) return; // blocking test 1069 | if (TWI_BUFFER_LENGTH < ptwv -> length) { 1070 | twi_Finish(1); // end write with error 1 1071 | return; 1072 | } 1073 | twi_Done = 0x00; // show as working 1074 | twii_SetState(TWI_MTX); // to transmitting 1075 | twii_SetError(0xFF); // to No Error 1076 | twii_InitBuffer(0, ptwv -> length); // pointer and length 1077 | twii_CopyToBuf(ptwv -> data, ptwv -> length); // get the data 1078 | twii_SetSlaRW((ptwv -> address << 1) | TW_WRITE); // write command 1079 | twii_SetStart(); // start the cycle 1080 | fNextInterruptFunction = twi_write01; // next routine 1081 | return twi_write01(); 1082 | } 1083 | 1084 | void twi_writeTo(uint8_t address, uint8_t* data, uint8_t length, uint8_t wait) { 1085 | uint8_t i; 1086 | ptwv = (twi_Write_Vars *)malloc(sizeof(twi_Write_Vars)); 1087 | ptwv -> address = address; 1088 | ptwv -> data = data; 1089 | ptwv -> length = length; 1090 | ptwv -> wait = wait; 1091 | fNextInterruptFunction = twi_write00; 1092 | return twi_write00(); 1093 | } 1094 | 1095 | void twi_read01() { 1096 | if (TWI_MRX == twi_state) return; // blocking test 1097 | if (twi_masterBufferIndex < ptwv -> length) ptwv -> length = twi_masterBufferIndex; 1098 | twii_CopyFromBuf(ptwv -> data, ptwv -> length); 1099 | twi_Finish(ptwv -> length); 1100 | if (twi_cbreadFromDone) return twi_cbreadFromDone(twi_Return_Value); 1101 | return; 1102 | } 1103 | 1104 | void twi_read00() { 1105 | if (TWI_READY != twi_state) return; // blocking test 1106 | if (TWI_BUFFER_LENGTH < ptwv -> length) twi_Finish(0); // error return 1107 | twi_Done = 0x00; // show as working 1108 | twii_SetState(TWI_MRX); // reading 1109 | twii_SetError(0xFF); // reset error 1110 | twii_InitBuffer(0, ptwv -> length - 1); // init to one less than length 1111 | twii_SetSlaRW((ptwv -> address << 1) | TW_READ); // read command 1112 | twii_SetStart(); // start cycle 1113 | fNextInterruptFunction = twi_read01; 1114 | return twi_read01(); 1115 | } 1116 | 1117 | void twi_readFrom(uint8_t address, uint8_t* data, uint8_t length) { 1118 | uint8_t i; 1119 | 1120 | ptwv = (twi_Write_Vars *)malloc(sizeof(twi_Write_Vars)); 1121 | ptwv -> address = address; 1122 | ptwv -> data = data; 1123 | ptwv -> length = length; 1124 | fNextInterruptFunction = twi_read00; 1125 | return twi_read00(); 1126 | } 1127 | 1128 | void twi_reply(uint8_t ack) { 1129 | // transmit master read ready signal, with or without ack 1130 | if (ack){ 1131 | TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWINT) | _BV(TWEA); 1132 | } else { 1133 | TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWINT); 1134 | } 1135 | } 1136 | 1137 | void twi_stop(void) { 1138 | // send stop condition 1139 | TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA) | _BV(TWINT) | _BV(TWSTO); 1140 | 1141 | // wait for stop condition to be exectued on bus 1142 | // TWINT is not set after a stop condition! 1143 | while (TWCR & _BV(TWSTO)) { 1144 | continue; 1145 | } 1146 | 1147 | // update twi state 1148 | twi_state = TWI_READY; 1149 | } 1150 | 1151 | void twi_releaseBus(void) { 1152 | // release bus 1153 | TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA) | _BV(TWINT); 1154 | 1155 | // update twi state 1156 | twi_state = TWI_READY; 1157 | } 1158 | 1159 | SIGNAL(TWI_vect) { 1160 | switch (TW_STATUS) { 1161 | // All Master 1162 | case TW_START: // sent start condition 1163 | case TW_REP_START: // sent repeated start condition 1164 | // copy device address and r/w bit to output register and ack 1165 | TWDR = twi_slarw; 1166 | twi_reply(1); 1167 | break; 1168 | 1169 | // Master Transmitter 1170 | case TW_MT_SLA_ACK: // slave receiver acked address 1171 | case TW_MT_DATA_ACK: // slave receiver acked data 1172 | // if there is data to send, send it, otherwise stop 1173 | if (twi_masterBufferIndex < twi_masterBufferLength) { 1174 | // copy data to output register and ack 1175 | TWDR = twi_masterBuffer[twi_masterBufferIndex++]; 1176 | twi_reply(1); 1177 | } else { 1178 | twi_stop(); 1179 | } 1180 | break; 1181 | 1182 | case TW_MT_SLA_NACK: // address sent, nack received 1183 | twi_error = TW_MT_SLA_NACK; 1184 | twi_stop(); 1185 | break; 1186 | 1187 | case TW_MT_DATA_NACK: // data sent, nack received 1188 | twi_error = TW_MT_DATA_NACK; 1189 | twi_stop(); 1190 | break; 1191 | 1192 | case TW_MT_ARB_LOST: // lost bus arbitration 1193 | twi_error = TW_MT_ARB_LOST; 1194 | twi_releaseBus(); 1195 | break; 1196 | 1197 | // Master Receiver 1198 | case TW_MR_DATA_ACK: // data received, ack sent 1199 | // put byte into buffer 1200 | twi_masterBuffer[twi_masterBufferIndex++] = TWDR; 1201 | 1202 | case TW_MR_SLA_ACK: // address sent, ack received 1203 | // ack if more bytes are expected, otherwise nack 1204 | if (twi_masterBufferIndex < twi_masterBufferLength) { 1205 | twi_reply(1); 1206 | } else { 1207 | twi_reply(0); 1208 | } 1209 | break; 1210 | 1211 | case TW_MR_DATA_NACK: // data received, nack sent 1212 | // put final byte into buffer 1213 | twi_masterBuffer[twi_masterBufferIndex++] = TWDR; 1214 | 1215 | case TW_MR_SLA_NACK: // address sent, nack received 1216 | twi_stop(); 1217 | break; 1218 | 1219 | // TW_MR_ARB_LOST handled by TW_MT_ARB_LOST case 1220 | 1221 | // Slave Receiver (NOT IMPLEMENTED YET) 1222 | /* 1223 | case TW_SR_SLA_ACK: // addressed, returned ack 1224 | case TW_SR_GCALL_ACK: // addressed generally, returned ack 1225 | case TW_SR_ARB_LOST_SLA_ACK: // lost arbitration, returned ack 1226 | case TW_SR_ARB_LOST_GCALL_ACK: // lost arbitration, returned ack 1227 | // enter slave receiver mode 1228 | twi_state = TWI_SRX; 1229 | 1230 | // indicate that rx buffer can be overwritten and ack 1231 | twi_rxBufferIndex = 0; 1232 | twi_reply(1); 1233 | break; 1234 | 1235 | case TW_SR_DATA_ACK: // data received, returned ack 1236 | case TW_SR_GCALL_DATA_ACK: // data received generally, returned ack 1237 | // if there is still room in the rx buffer 1238 | if (twi_rxBufferIndex < TWI_BUFFER_LENGTH) { 1239 | // put byte in buffer and ack 1240 | twi_rxBuffer[twi_rxBufferIndex++] = TWDR; 1241 | twi_reply(1); 1242 | } else { 1243 | // otherwise nack 1244 | twi_reply(0); 1245 | } 1246 | break; 1247 | 1248 | case TW_SR_STOP: // stop or repeated start condition received 1249 | // put a null char after data if there's room 1250 | if (twi_rxBufferIndex < TWI_BUFFER_LENGTH) { 1251 | twi_rxBuffer[twi_rxBufferIndex] = 0; 1252 | } 1253 | 1254 | // sends ack and stops interface for clock stretching 1255 | twi_stop(); 1256 | 1257 | // callback to user defined callback 1258 | twi_onSlaveReceive(twi_rxBuffer, twi_rxBufferIndex); 1259 | 1260 | // since we submit rx buffer to "wire" library, we can reset it 1261 | twi_rxBufferIndex = 0; 1262 | 1263 | // ack future responses and leave slave receiver state 1264 | twi_releaseBus(); 1265 | break; 1266 | 1267 | case TW_SR_DATA_NACK: // data received, returned nack 1268 | case TW_SR_GCALL_DATA_NACK: // data received generally, returned nack 1269 | // nack back at master 1270 | twi_reply(0); 1271 | break; 1272 | 1273 | // Slave Transmitter 1274 | case TW_ST_SLA_ACK: // addressed, returned ack 1275 | case TW_ST_ARB_LOST_SLA_ACK: // arbitration lost, returned ack 1276 | // enter slave transmitter mode 1277 | twi_state = TWI_STX; 1278 | 1279 | // ready the tx buffer index for iteration 1280 | twi_txBufferIndex = 0; 1281 | 1282 | // set tx buffer length to be zero, to verify if user changes it 1283 | twi_txBufferLength = 0; 1284 | 1285 | // request for txBuffer to be filled and length to be set 1286 | // note: user must call twi_transmit(bytes, length) to do this 1287 | twi_onSlaveTransmit(); 1288 | 1289 | // if they didn't change buffer & length, initialize it 1290 | if (0 == twi_txBufferLength) { 1291 | twi_txBufferLength = 1; 1292 | twi_txBuffer[0] = 0x00; 1293 | } 1294 | 1295 | // transmit first byte from buffer, fall through 1296 | 1297 | case TW_ST_DATA_ACK: // byte sent, ack returned 1298 | // copy data to output register 1299 | TWDR = twi_txBuffer[twi_txBufferIndex++]; 1300 | 1301 | // if there is more to send, ack, otherwise nack 1302 | if (twi_txBufferIndex < twi_txBufferLength) { 1303 | twi_reply(1); 1304 | } else { 1305 | twi_reply(0); 1306 | } 1307 | break; 1308 | 1309 | case TW_ST_DATA_NACK: // received nack, we are done 1310 | case TW_ST_LAST_DATA: // received ack, but we are done already! 1311 | // ack future responses 1312 | twi_reply(1); 1313 | // leave slave receiver state 1314 | twi_state = TWI_READY; 1315 | break; 1316 | */ 1317 | 1318 | // all 1319 | case TW_NO_INFO: // no state information 1320 | break; 1321 | 1322 | case TW_BUS_ERROR: // bus error, illegal stop/start 1323 | twi_error = TW_BUS_ERROR; 1324 | twi_stop(); 1325 | break; 1326 | } 1327 | 1328 | if (fNextInterruptFunction) return fNextInterruptFunction(); 1329 | } 1330 | 1331 | TwoWire::TwoWire() { } 1332 | 1333 | void TwoWire::begin(void) { 1334 | rxBufferIndex = 0; 1335 | rxBufferLength = 0; 1336 | 1337 | txBufferIndex = 0; 1338 | txBufferLength = 0; 1339 | 1340 | twi_init(); 1341 | } 1342 | 1343 | void TwoWire::beginTransmission(uint8_t address) { 1344 | //beginTransmission((uint8_t)address); 1345 | 1346 | // indicate that we are transmitting 1347 | twi_transmitting = 1; 1348 | 1349 | // set address of targeted slave 1350 | txAddress = address; 1351 | 1352 | // reset tx buffer iterator vars 1353 | txBufferIndex = 0; 1354 | txBufferLength = 0; 1355 | } 1356 | 1357 | uint8_t TwoWire::endTransmission(uint16_t timeout) { 1358 | // transmit buffer (blocking) 1359 | //int8_t ret = 1360 | twi_cbendTransmissionDone = NULL; 1361 | twi_writeTo(txAddress, txBuffer, txBufferLength, 1); 1362 | int8_t ret = twii_WaitForDone(timeout); 1363 | 1364 | // reset tx buffer iterator vars 1365 | txBufferIndex = 0; 1366 | txBufferLength = 0; 1367 | 1368 | // indicate that we are done transmitting 1369 | // twi_transmitting = 0; 1370 | return ret; 1371 | } 1372 | 1373 | void TwoWire::nbendTransmission(void (*function)(int)) { 1374 | twi_cbendTransmissionDone = function; 1375 | twi_writeTo(txAddress, txBuffer, txBufferLength, 1); 1376 | return; 1377 | } 1378 | 1379 | void TwoWire::send(uint8_t data) { 1380 | if (twi_transmitting) { 1381 | // in master transmitter mode 1382 | // don't bother if buffer is full 1383 | if (txBufferLength >= NBWIRE_BUFFER_LENGTH) { 1384 | return; 1385 | } 1386 | 1387 | // put byte in tx buffer 1388 | txBuffer[txBufferIndex] = data; 1389 | ++txBufferIndex; 1390 | 1391 | // update amount in buffer 1392 | txBufferLength = txBufferIndex; 1393 | } else { 1394 | // in slave send mode 1395 | // reply to master 1396 | //twi_transmit(&data, 1); 1397 | } 1398 | } 1399 | 1400 | uint8_t TwoWire::receive(void) { 1401 | // default to returning null char 1402 | // for people using with char strings 1403 | uint8_t value = 0; 1404 | 1405 | // get each successive byte on each call 1406 | if (rxBufferIndex < rxBufferLength) { 1407 | value = rxBuffer[rxBufferIndex]; 1408 | ++rxBufferIndex; 1409 | } 1410 | 1411 | return value; 1412 | } 1413 | 1414 | uint8_t TwoWire::requestFrom(uint8_t address, int quantity, uint16_t timeout) { 1415 | // clamp to buffer length 1416 | if (quantity > NBWIRE_BUFFER_LENGTH) { 1417 | quantity = NBWIRE_BUFFER_LENGTH; 1418 | } 1419 | 1420 | // perform blocking read into buffer 1421 | twi_cbreadFromDone = NULL; 1422 | twi_readFrom(address, rxBuffer, quantity); 1423 | uint8_t read = twii_WaitForDone(timeout); 1424 | 1425 | // set rx buffer iterator vars 1426 | rxBufferIndex = 0; 1427 | rxBufferLength = read; 1428 | 1429 | return read; 1430 | } 1431 | 1432 | void TwoWire::nbrequestFrom(uint8_t address, int quantity, void (*function)(int)) { 1433 | // clamp to buffer length 1434 | if (quantity > NBWIRE_BUFFER_LENGTH) { 1435 | quantity = NBWIRE_BUFFER_LENGTH; 1436 | } 1437 | 1438 | // perform blocking read into buffer 1439 | twi_cbreadFromDone = function; 1440 | twi_readFrom(address, rxBuffer, quantity); 1441 | //uint8_t read = twii_WaitForDone(); 1442 | 1443 | // set rx buffer iterator vars 1444 | //rxBufferIndex = 0; 1445 | //rxBufferLength = read; 1446 | 1447 | rxBufferIndex = 0; 1448 | rxBufferLength = quantity; // this is a hack 1449 | 1450 | return; //read; 1451 | } 1452 | 1453 | uint8_t TwoWire::available(void) { 1454 | return rxBufferLength - rxBufferIndex; 1455 | } 1456 | 1457 | #endif 1458 | --------------------------------------------------------------------------------