├── AnalogIn.h ├── ArduinoSensors.h ├── DigitalIn.h ├── DigitalOut.h ├── LICENSE ├── README.md ├── examples └── Ultrasonic Sensors │ ├── HCSR04 │ └── HCSR04.ino │ └── Ping │ └── Ping.ino ├── interfaces ├── AngleInterface.h ├── DistanceInterface.h ├── TemperatureInterface.h └── VoltageInterface.h ├── library.properties └── sensors ├── AnalogVoltage.h ├── CompoundDistanceSensor.h ├── EZUltrasonicAnalog.h ├── HMC6352.h ├── MLX90614.h ├── MPU ├── MPU9150.cpp ├── MPU9150.h ├── dmpMPU6050.h ├── dmpMPU9150.h └── helper_3dmath.h ├── MPUSensor.h ├── PingUltrasonic.h ├── SharpInterface.h ├── SharpLong.h ├── SharpMedium.h └── SharpShort.h /AnalogIn.h: -------------------------------------------------------------------------------- 1 | /* 2 | AnalogIn.h - A class to help with Analog Input pins. 3 | 4 | https://github.com/ivanseidel/Arduino-Sensors 5 | 6 | Created by Ivan Seidel Gomes, June, 2013. 7 | Released into the public domain. 8 | */ 9 | 10 | #ifndef AnalogIn_h 11 | #define AnalogIn_h 12 | 13 | #include "Arduino.h" 14 | 15 | #include 16 | 17 | class AnalogIn: public Thread 18 | { 19 | protected: 20 | // Cached value of the AnalogInput 21 | long value; 22 | 23 | // Threshold value 24 | long threshold; 25 | 26 | // Pin to read to 27 | int inPin; 28 | 29 | public: 30 | 31 | /* 32 | Initialize the Input pin. 33 | 34 | _inPin: Analog input pin to read value 35 | */ 36 | AnalogIn(int _inPin = -1){ 37 | setup(_inPin); 38 | 39 | Thread::Thread(); 40 | setInterval(20); 41 | } 42 | 43 | virtual void setup(int _inPin){ 44 | if(_inPin < 0) 45 | return; 46 | 47 | inPin = _inPin; 48 | 49 | pinMode(inPin, INPUT); 50 | 51 | value = 0; 52 | threshold = 0; 53 | } 54 | 55 | /* 56 | Use this operator to compare with boolean values. 57 | 58 | This will return if the threshold is or not equeal to 59 | the other boolean. 60 | 61 | Eg.: (inputPin == HIGH) 62 | is the same as 63 | (inputPin.aboveThreshold() == true) 64 | */ 65 | virtual bool operator==(bool other){ 66 | return aboveThreshold() == other; 67 | } 68 | 69 | /* 70 | Called when this Thread is runned 71 | */ 72 | virtual void run(){ 73 | read(); 74 | runned(); 75 | } 76 | 77 | /* 78 | Reads the ADC and avarage it three times (reduce noise) 79 | Saves localy and then returns it. 80 | */ 81 | virtual long read(){ 82 | value = (analogRead(inPin) + analogRead(inPin))/2; 83 | return getValue(); 84 | } 85 | 86 | /* 87 | Only returns the value, it doesen't samples the ADC 88 | */ 89 | virtual long getValue(){ 90 | return value; 91 | } 92 | 93 | /* 94 | Set a threshold for comparation 95 | */ 96 | virtual void setThreshold(long _threshold){ 97 | threshold = _threshold; 98 | } 99 | 100 | /* 101 | Check if it's avove the selected threshold 102 | */ 103 | virtual bool aboveThreshold(){ 104 | return read() > threshold; 105 | } 106 | 107 | /* 108 | Check if it's below the selected threshold 109 | */ 110 | virtual bool belowThreshold(){ 111 | return read() < threshold; 112 | } 113 | 114 | }; 115 | 116 | #endif -------------------------------------------------------------------------------- /ArduinoSensors.h: -------------------------------------------------------------------------------- 1 | /* 2 | ArduinoSensors.h - Main ArduinoSensor file. 3 | 4 | Provides repetitive code in pretty functions 5 | 6 | Include this file in order to allow the compiler to discover the sub 7 | folders and files of ArduinoSensors Library 8 | 9 | For instructions, go to https://github.com/ivanseidel/Arduino-Sensors 10 | 11 | Created by Ivan Seidel Gomes, June, 2013. 12 | Released into the public domain. 13 | */ 14 | 15 | 16 | #ifndef ArduinoSensors_h 17 | #define ArduinoSensors_h 18 | 19 | #include 20 | 21 | /* 22 | Simple conversion from float to String. 23 | 24 | float f: the float to be converted 25 | int decimals: number of decimal places 26 | */ 27 | String floatToString(float f, int decimals = 2){ 28 | /* 29 | Transforms the floating point string process: 30 | eg.: using decimals = 2, input: 0.01 31 | 1.23456 * (pow(10, 2+1) 32 | 1.23456 * 1000 = 1234 (as a long) 33 | now, let's move digits asside: 34 | 3 goes to 4, 2 goes to 3. 35 | Stages: "1234" 36 | "1233" 37 | "1223" 38 | Not, put a dot in the place of the separator 39 | "1.23" 40 | 41 | */ 42 | long fLong = f*(pow(10,decimals+1)); 43 | String longString = String(fLong); 44 | int len = longString.length(); 45 | 46 | for(int i = 0; i < decimals; i++){ 47 | longString[len - 1 - i] = longString[len - 2 - i]; 48 | } 49 | longString[len - decimals - 1] = '.'; 50 | 51 | return longString; 52 | } 53 | 54 | /* 55 | Convert an angle in Degrees to Radians 56 | */ 57 | float toRads(double angle){ 58 | return angle * M_PI/180.0; 59 | } 60 | 61 | /* 62 | Convert an angle in Radians to Degrees 63 | */ 64 | float toDegs(double angle){ 65 | return angle * 180.0/M_PI; 66 | } 67 | 68 | /* 69 | This function is usefull for fixing realtive angles. 70 | If you input anything from -180 to 180, it will return the same. 71 | 72 | However, if you input for example, 200, it will return -160 73 | Input | Output 74 | 0 0 75 | 180 180 76 | -180 -180 77 | 181 -179 78 | -181 179 79 | 360 0 80 | 540 -180 81 | 82 | */ 83 | float fixDegrees(float angle){ 84 | while (angle < -180.0) 85 | angle += 360.0; 86 | 87 | while (angle > 180.0) 88 | angle -= 360.0; 89 | 90 | return angle; 91 | }; 92 | 93 | 94 | /* 95 | This is the same as the above, but working with Radians. 96 | (PI = 180; -PI = -180; 2*PI = 360) 97 | */ 98 | float fixRads(float angle){ 99 | while (angle < - M_PI) 100 | angle += 2*M_PI; 101 | 102 | while (angle > M_PI) 103 | angle -= 2*M_PI; 104 | 105 | return angle; 106 | }; 107 | 108 | /* 109 | Converts from inches to centimeters 110 | */ 111 | inline float toCentimeters(float inches){ 112 | return inches * 0.393700787; 113 | } 114 | 115 | /* 116 | Converts from centimeters to inches 117 | */ 118 | inline float toInches(float centimeters){ 119 | return centimeters * 2.54; 120 | } 121 | 122 | 123 | /* 124 | This preprocessor logic will determine the default 125 | AREF voltage for the current Arduino being compiled 126 | */ 127 | #if defined(__AVR__) 128 | 129 | /* 130 | AVR family usualy is 3.3v when running on 8Mhz, 131 | and 5.0 if running on 16Mhz 132 | */ 133 | #if F_CPU <= 8000000 134 | #warning "ADC_DEFAULT_AREF = 3.3v | AVR <=8Mhz clock" 135 | #define ADC_DEFAULT_AREF 3.3 136 | #elif F_CPU <= 16000000 137 | #warning "ADC_DEFAULT_AREF = 5.0v | AVR >8Mhz & <=16Mhz clock" 138 | #define ADC_DEFAULT_AREF 5.0 139 | #else 140 | #warning "ADC_DEFAULT_AREF = 5.0v | AVR Default AREF" 141 | #define ADC_DEFAULT_AREF 5.0 142 | #endif 143 | 144 | #elif defined(__PIC32MX__) 145 | 146 | /* 147 | Currently, all Arduinos with PIC32 are 3.3v 148 | */ 149 | #pragma message("ADC_DEFAULT_AREF = 3.3v | PIC32 Archtecture") 150 | #define ADC_DEFAULT_AREF 3.3 151 | 152 | #elif defined(__arm__) 153 | 154 | /* 155 | Currently, all Arduinos with ARM are 3.3v 156 | */ 157 | #pragma message("ADC_DEFAULT_AREF = 3.3v | ARM Archtecture") 158 | #define ADC_DEFAULT_AREF 3.3 159 | 160 | #endif 161 | 162 | #ifndef ADC_DEFAULT_AREF 163 | #warning "ADC_DEFAULT_AREF = 5.0v | Default value" 164 | #define ADC_DEFAULT_AREF 5.0 165 | #endif 166 | 167 | /* 168 | With tests, I could notice an average voltage output 169 | from the regulator of (VCC * (1 - 0.03)) as the output. 170 | 171 | An average of 3% drop is the normal. 172 | 173 | So we define that to fix some small erros. 174 | 175 | NOTE: This feature is disabled by default. 176 | */ 177 | // #define ADC_VOLTAGE_PROP 0.03 178 | #define ADC_VOLTAGE_PROP 0.00 179 | 180 | #endif -------------------------------------------------------------------------------- /DigitalIn.h: -------------------------------------------------------------------------------- 1 | /* 2 | DigitalIn.h - A class to help with Digital Input pins. 3 | 4 | Instantiate: 5 | DigitalIn myInputPin(13); 6 | 7 | Check state: 8 | if(myOutputPin == HIGH); 9 | if(myOutputPin.getValue() == HIGH); 10 | 11 | For instructions, go to https://github.com/ivanseidel/Arduino-Sensors 12 | 13 | Created by Ivan Seidel Gomes, June, 2013. 14 | Released into the public domain. 15 | */ 16 | 17 | #ifndef DigitalIn_h 18 | #define DigitalIn_h 19 | 20 | #include "Arduino.h" 21 | 22 | class DigitalIn 23 | { 24 | protected: 25 | /* 26 | The number of the pin 27 | */ 28 | int pin; 29 | 30 | /* 31 | This attribute is a flag to indicate if 32 | the output will be inverted. 33 | 34 | If set to true, then turning "ON" actualy 35 | turns OFF. 36 | 37 | (Eg. of usage: Relays that activate on LOW) 38 | */ 39 | bool invert; 40 | 41 | // Cached value for the current pin 42 | bool digitalValue; 43 | 44 | public: 45 | 46 | /* 47 | Pin: Analog pin to read value 48 | Invert: Should invert the output signal? 49 | */ 50 | DigitalIn(int _outPin, bool _invert = false){ 51 | pin = _outPin; 52 | invert = _invert; 53 | 54 | pinMode(pin, INPUT); 55 | } 56 | 57 | /* 58 | Compares if the output signal of this pin, 59 | is equal to other boolean value 60 | */ 61 | virtual operator bool(){ 62 | return readValue(); 63 | } 64 | 65 | /* 66 | Returns the current state of the digital pin 67 | */ 68 | virtual bool readValue(){ 69 | digitalValue = (digitalRead(pin) ^ invert); 70 | return getValue(); 71 | } 72 | 73 | /* 74 | Returns the cached state of the digital pin 75 | */ 76 | virtual bool getValue(){ 77 | return digitalValue; 78 | } 79 | }; 80 | 81 | #endif -------------------------------------------------------------------------------- /DigitalOut.h: -------------------------------------------------------------------------------- 1 | /* 2 | DigitalOut.h - A class to help with Digital Output pins. 3 | 4 | Instantiate: 5 | DigitalOut myOutputPin(13); 6 | 7 | Set output: 8 | myOutputPin = HIGH; 9 | myOutputPin.turn(HIGH); 10 | myOutputPin.turnOn(); 11 | 12 | For instructions, go to https://github.com/ivanseidel/Arduino-Sensors 13 | 14 | Created by Ivan Seidel Gomes, June, 2013. 15 | Released into the public domain. 16 | */ 17 | 18 | #ifndef DigitalOut_h 19 | #define DigitalOut_h 20 | 21 | #include "Arduino.h" 22 | 23 | class DigitalOut 24 | { 25 | protected: 26 | /* 27 | The number of the pin 28 | */ 29 | int outPin; 30 | 31 | /* 32 | This attribute is a flag to indicate if 33 | the output will be inverted. 34 | 35 | If set to true, then turning "ON" actualy 36 | turns OFF. 37 | 38 | (Eg. of usage: Relays that activate on LOW) 39 | */ 40 | bool invert; 41 | 42 | public: 43 | 44 | /* 45 | Pin: Analog pin to read value 46 | Invert: Should invert the output signal? 47 | */ 48 | DigitalOut(int _outPin, bool _invert = false){ 49 | pinMode(_outPin, OUTPUT); 50 | 51 | outPin = _outPin; 52 | invert = _invert; 53 | 54 | turn(LOW); 55 | } 56 | 57 | /* 58 | Return the output signal of this pin 59 | */ 60 | virtual operator bool(){ 61 | return (digitalRead(outPin) ^ invert); 62 | } 63 | 64 | /* 65 | Set the output to the desired value; 66 | Use as: 67 | digitalOutObject = true; (Turns on) 68 | digitalOutObject = HIGH; (Turns on) 69 | digitalOutObject = LOW; (Turns off) 70 | */ 71 | virtual void operator=(bool on){ 72 | turn(on); 73 | } 74 | 75 | /* 76 | Set the output to HIGH or LOW depending on the boolean on 77 | and the invert attribute 78 | */ 79 | virtual void turn(bool on){ 80 | #if defined(__arm__) 81 | // A little bit faster than digitalWrite 82 | PIO_SetOutput( g_APinDescription[outPin].pPort, g_APinDescription[outPin].ulPin, on ^ invert, 0, PIO_PULLUP ) ; 83 | #else 84 | digitalWrite(outPin, on ^ invert); 85 | #endif 86 | } 87 | 88 | /* 89 | Facilitate methods 90 | */ 91 | virtual void turnOn(){ 92 | turn(HIGH); 93 | } 94 | virtual void turnOff(){ 95 | turn(LOW); 96 | } 97 | 98 | }; 99 | 100 | #endif -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | The MIT License (MIT) 2 | 3 | Copyright (c) 2013 Ivan Seidel 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy of 6 | this software and associated documentation files (the "Software"), to deal in 7 | the Software without restriction, including without limitation the rights to 8 | use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 9 | the Software, and to permit persons to whom the Software is furnished to do so, 10 | subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 17 | FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 18 | COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 19 | IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 20 | CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 21 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Arduino-Sensors 2 | 3 | A "Library" of Libraries of Sensors. Infrared, Ultrasonic, Compass and many others, 4 | ready to work with [ArduinoThread](https://github.com/ivanseidel/ArduinoThread) and fully object oriented. 5 | 6 | This library was made to provide lot's of out-of-the-box features, such as 'Interfaces', 'Threadable' sensors, 7 | and reliability over C++ Objects. 8 | 9 | If you are a developer, please consider developing more sensor Libraries following the given pattern. I will be 10 | pleased to incorporate it to the library! 11 | 12 | ## Installation 13 | 14 | 1. [Download](https://github.com/ivanseidel/ArduinoSensors/archive/master.zip) the Latest release from gitHub. 15 | 2. Unzip and modify the Folder name to "ArduinoSensors" (Remove the '-version') 16 | 3. Paste the modified folder on your Library folder (On your `Libraries` folder inside Sketchbooks or Arduino software). 17 | 4. Download and Install [ArduinoThread](https://github.com/ivanseidel/ArduinoThread) 18 | 5. Restart the Arduino IDE 19 | 20 | ## Included Generic Classes: 21 | 22 | * AnalogIn 23 | * DigitalIn 24 | * DigitalOut 25 | * AnalogVoltage 26 | 27 | ## Included Sensor Classes: 28 | 29 | * MLX90614 Temperature Sensor 30 | * HMC6352 Compass sensor 31 | * EZ Ultrasonic Sensor 32 | * Ping Ultrasonic Sensor 33 | * HC-SR04 Ultrasonic Sensor 34 | * InvenSense IMU MPU9150 35 | * InvenSense IMU MPU6050 36 | * Sharp Infrared Short Distance Sensor (GP2D120XJ00F) 37 | * Sharp Infrared Medium Distance Sensor (GP2Y0A21YK) 38 | * Sharp Infrared Long Distance Sensor (GP2Y0A02YK0F) 39 | 40 | # The Concept of this Library 41 | 42 | If you are here, you are probably looking for some sensor library. Perhaps an Distance sensor, or a Compass sensor, or even a IMU sensor. What do they all have in common? They are all sensors, and do some measurement. Perhaps an Angle, or a Distance, or a Speed... 43 | 44 | Some libraries uses functions and others use Classes witch are better to maitain and even to understand the code. This Library tries to create a patterns that allows others to create better Sensor libraries with it, while keeping method names generic and compatibility across multiple platforms. Also, Sensors extend from it's base the a Thread (from [ArduinoThread](https://github.com/ivanseidel/ArduinoThread) Library), allowing `async` fetch of sensor data pretty easily. 45 | 46 | But to do all that, we must have a strong and nice Base to work with. For example: There are many platforms that are 3.3v, 5v and even 1.8V. How can we make it work with multiple ADC values? How can we make it simple to develop and to use? Continue reading and you will understand. 47 | 48 | # Base Classes 49 | 50 | Consists of a set of classes that helps another classes like `DigitalIn`, `AnalogIn`, `DigitalOut`, `AnalogVoltage`. 51 | 52 | Also contains a set of Interfaces such as `AngleInterface`, `DistanceInterface`, `TemperatureInterface`, `VoltageInterface`, used by different sensor types. 53 | 54 | Let's say we have a new Temperature sensor, and we want to create a Library for it. What we expect to have: 55 | * **Generic method names.** In this case, it would extend a `TemperatureInterface`, that has the method `getTemperature` and `readTemperature` 56 | * **Caching it's value.** We should be able to both `read` it's value (actually fetch sensor data, read analog, communicate...), or `get` it's current value (basically, return what was read last, like a caching). 57 | * **Work Asynchronously.** Implementing Thread from [ArduinoThread](https://github.com/ivanseidel/ArduinoThread) Library, we can make it read with a fixed timing, and get it's value latter without further delay. 58 | 59 | ## Interfaces 60 | 61 | Interfaces are a very good way of transforming your usual code into a more generic way. It's basicaly a set of methods and properties (variables), that a Class will inherit. For example, every class that extends `AngleInterface`, will have a method called `readAngle` and `getAngle`. If a new sensor library is created, that reads an angle (perhaps a potentiometer, Compass or even a encoder), it could inherit from this interface, and get those methods. 62 | 63 | I will explain why it's good to use with an experience I had: 64 | 65 | I was developing a complex robot, that would require multiple Distance Sensor types. That included two types of Infrared distance sensor, and one type of Ultrasonic sensor. I have developed this library thinking that whenever I needed, I could change the sensor in hardware for another, and it would still work. Shure that there are physical differences between them, but what I wanted was to work with multiple sensors without altering more than ONE line of code. I so I did it. We could test with different sensors, without taking so much time on software and also hardware. 66 | 67 | We also used a IMU from Invensense, that has a Compass. After a while, we decide to change that for a simple Compass Sensor. Both of the sensors libraries was made by me, implementing it's `AngleInterface`. 68 | 69 | ## Base Classes 70 | 71 | Base Classes are used by other classes, and also by you (if you want), providing a even more High Level approach for IO manipulation. 72 | 73 | ### DigitalIn Class 74 | 75 | Provides a easy approach for manipulating a Digital Input pin: 76 | 77 | ``` 78 | DigitalIn myButton(9); 79 | 80 | // Check if Pin is HIGH 81 | if (myButton)... 82 | 83 | // Check if Pin is LOW 84 | if (myButton == LOW)... 85 | 86 | // Reads it's value 87 | boolean val = myButton.readValue(); 88 | 89 | // Get last read value 90 | val = myButton.getValue(); 91 | ``` 92 | 93 | Also, if you initialize it passing `true` as a seccond param, every read will be inverted: 94 | ``` 95 | DigitalIn myInvertedButton(10, true); 96 | 97 | // Check if Pin is LOW 98 | if(myInvertedButton) 99 | 100 | // Read it's value (inverted) 101 | myInvertedButton.readValue(); 102 | ``` 103 | 104 | ### DigitalOut Class 105 | 106 | Used to manipulate Digital Output pins in a easy way. 107 | 108 | ``` 109 | DigitalOut myLed(13); 110 | 111 | // Turns LED on 112 | myLed = HIGH; 113 | myLed.turn(HIGH); 114 | myLed.turnOn(); 115 | 116 | // Turns LED off 117 | myLed = LOW; 118 | myLed.turn(LOW); 119 | myLed.turnOff(); 120 | ``` 121 | 122 | Like DigitalIn, if you assign an seccond parameter in it's initialization, it will automatically invert it's output 123 | ``` 124 | DigitalOut myLedInverted(13, true); 125 | 126 | // Turns LED off 127 | myLedInverted = HIGH 128 | 129 | // You can also check it's istate like this: 130 | if(myLedInverted == HIGH) ... 131 | ``` 132 | 133 | ### AnalogIn Class 134 | 135 | We usually need to use Analog Pins as Digital. Or even Read it's value multiple times. I usually consider a lot the timing of things, knowing every milisecond that is happening on my program. Because of that you might consider using this class instead of usual `analogRead` function. 136 | 137 | ``` 138 | AnalogIn myPot(A0); 139 | 140 | // Reads it's value 141 | val = myPot.read(); 142 | 143 | // Get it's cached value (does not reads ADC, only returns last value) 144 | val = myPot.getValue(); 145 | ``` 146 | 147 | One usual case, is setting a threshold to compare. We do it for you: 148 | 149 | ``` 150 | // Set threshold for 500 151 | myPot.setThreshold(500); 152 | 153 | // Check if below 500 154 | if(myPot.belowThreshold())... 155 | 156 | if(myPot == false)... 157 | 158 | // Check if above 500 159 | if(myPot.aboveThreshold())... 160 | 161 | if(myPot == true)... 162 | ``` 163 | 164 | ## AnalogVoltage Class 165 | 166 | This is a nice step for next generations of Libraries, because there are many Analog Sensors around, that it's converted value should be mapped to a Voltage, not a ADC value. But that's a problem becoming larger today, since there are platforms that runs with 5v, others with 3.3v... And imagine, every single library thrying to work whit that (or not)? 167 | 168 | ``` 169 | AnalogVoltage myVoltimeter(A0); 170 | 171 | // Read the voltage (in Volts) 172 | float pinVoltage = myVoltimeter.readVoltage(); 173 | ``` 174 | 175 | Well, you might be wondering: How does it do it's magic? The answer is pretty simple: **Preprocessor macros**. 176 | 177 | In compiling time, we detect weather you are using a 3.3v or 5v platform, and set a definition called `ADC_DEFAULT_AREF`. We use it later to scale the ADC to a voltage. But keep calm, you can override that pretty easily, and allows you to create custom `linear` voltage in readers (Like a voltage divider, to read a battery voltage). 178 | 179 | ``` 180 | // 0 in ADC now means -5v. full ADC (1024) means 10v 181 | AnalogVoltage myCustomVoltimeter(A0, -5, 10); 182 | 183 | float pinVoltage = myCustomVoltimeter.readVoltage(); 184 | ``` 185 | 186 | # Working toguether with ArduinoThread 187 | 188 | Read more about [ArduinoThread](https://github.com/ivanseidel/ArduinoThread) here. It's a library I created to simplify complex softwares that requires multiple async tasks. Let's say: 189 | * Read Compass every 50ms 190 | * Read Distance Sensors every 70ms 191 | * Read a button every 5ms 192 | * Get battery voltage every 1 minute 193 | * and so on... 194 | 195 | If you want a REAL example of it working, take a look at our [2013 RoboCup Junior Soccer Robot code](https://github.com/ivanseidel/Robot-Soccer-2013/tree/master/Software/Soccer_v02). 196 | 197 | The idea is that you can register Threads in ThreadController (a group of Threads), and let the system check if it should be runned. In that way, you just GET sensor values in your main code, without consuming time every time you need something. 198 | 199 | ``` 200 | // Create a new ThreadController that will run always 201 | // (means it will check if it's threads should be runned everytime) 202 | ThreadController threads(0); 203 | 204 | // Create some sensors 205 | DistanceInterface *myDist1 = new SharpLong(A0); 206 | DistanceInterface *myDist2 = new SharpShort(A1); 207 | TemperatureInterface *thermometer = new MLX90614(0x32); 208 | AngleInterface *myCompass = new HMC6352(); 209 | 210 | // Each sensor has it's interval set to minimum possible 211 | // or something good to work with (20ms - 70ms). 212 | 213 | // But they ARE Thread objects, so you can make use of 214 | // it's methods: 215 | 216 | // Compass should be read every 60ms 217 | myCompass.setInterval(60); 218 | 219 | // Add to the thread controller 220 | threads.add(&myDist1); 221 | threads.add(&myDist2); 222 | threads.add(&thermometer); 223 | threads.add(&myCompass); 224 | 225 | // Now, just call threads.run(); to automatically run what is needed 226 | // (perhaps in a Timer Callback? or in Loop.. whatever) 227 | loop(){ 228 | threads.run(); 229 | 230 | // Get the most recend value WITHOUT reading the sensor 231 | double currentAngle = myCompass->getAngle(); 232 | float distance = myDist1->getDistance(); 233 | [...] 234 | } 235 | 236 | ``` 237 | 238 | ## Rest of Documentation under development... 239 | -------------------------------------------------------------------------------- /examples/Ultrasonic Sensors/HCSR04/HCSR04.ino: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | /* 6 | Echo and Trigger pins must be the same 7 | to your connections on the arduino 8 | */ 9 | int echoPin = 12; 10 | int triggerPin = 13; 11 | 12 | PingUltrasonic myUltrasonic(triggerPin, echoPin); 13 | 14 | void setup() { 15 | Serial.begin(9600); 16 | Serial.println("I'm alive!"); 17 | } 18 | 19 | void loop() 20 | { 21 | float distance = myUltrasonic.readDistance(); 22 | 23 | Serial.print("Distance: "); 24 | Serial.print(distance); 25 | Serial.println("cm"); 26 | 27 | delay(100); 28 | } 29 | -------------------------------------------------------------------------------- /examples/Ultrasonic Sensors/Ping/Ping.ino: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | /* 6 | Signal pin must be the same 7 | to your connection on the arduino 8 | */ 9 | int sigPin = 12; 10 | 11 | PingUltrasonic myUltrasonic(sigPin); 12 | 13 | void setup() { 14 | Serial.begin(9600); 15 | Serial.println("I'm alive!"); 16 | } 17 | 18 | void loop() 19 | { 20 | float distance = myUltrasonic.readDistance(); 21 | 22 | Serial.print("Distance: "); 23 | Serial.print(distance); 24 | Serial.println("cm"); 25 | 26 | delay(100); 27 | } -------------------------------------------------------------------------------- /interfaces/AngleInterface.h: -------------------------------------------------------------------------------- 1 | /* 2 | AngleInterface.h - An Interface for sensors that measure Angle 3 | 4 | For instructions, go to https://github.com/ivanseidel/Arduino-Sensors 5 | 6 | Created by Ivan Seidel Gomes, June, 2013. 7 | Released into the public domain. 8 | */ 9 | 10 | #ifndef AngleInterface_h 11 | #define AngleInterface_h 12 | 13 | class AngleInterface 14 | { 15 | public: 16 | // Should return the CACHED value of the sensor 17 | virtual float getAngle(); 18 | 19 | // Should READ and CACHE the value of the sensor 20 | virtual float readAngle(); 21 | }; 22 | 23 | #endif -------------------------------------------------------------------------------- /interfaces/DistanceInterface.h: -------------------------------------------------------------------------------- 1 | /* 2 | DistanceInterface.h - An Interface for sensors that measure Distance 3 | 4 | For instructions, go to https://github.com/ivanseidel/Arduino-Sensors 5 | 6 | Created by Ivan Seidel Gomes, June, 2013. 7 | Released into the public domain. 8 | */ 9 | 10 | #ifndef DistanceInterface_h 11 | #define DistanceInterface_h 12 | 13 | class DistanceInterface 14 | { 15 | public: 16 | // This is the minimum range of the sensor 17 | float minDistance; 18 | // This is the maximum range of the sensor 19 | float maxDistance; 20 | 21 | // Should return the CACHED value of the sensor 22 | virtual float getDistance(); 23 | 24 | // Should READ and CACHE the value of the sensor 25 | virtual float readDistance(); 26 | }; 27 | 28 | #endif -------------------------------------------------------------------------------- /interfaces/TemperatureInterface.h: -------------------------------------------------------------------------------- 1 | /* 2 | TemperatureInterface.h - An Interface for sensors that measure Temperature 3 | 4 | For instructions, go to https://github.com/ivanseidel/Arduino-Sensors 5 | 6 | Created by Ivan Seidel Gomes, June, 2013. 7 | Released into the public domain. 8 | */ 9 | 10 | #ifndef TemperatureInterface_h 11 | #define TemperatureInterface_h 12 | 13 | class TemperatureInterface 14 | { 15 | public: 16 | // This is the minimum range of the sensor 17 | float minTemperature; 18 | // This is the maximum range of the sensor 19 | float maxTemperature; 20 | 21 | // Should return the CACHED value of the sensor 22 | virtual float getTemperature(); 23 | 24 | // Should READ and CACHE the value of the sensor 25 | virtual float readTemperature(); 26 | }; 27 | 28 | #endif -------------------------------------------------------------------------------- /interfaces/VoltageInterface.h: -------------------------------------------------------------------------------- 1 | /* 2 | VoltageInterface.h - An Interface for Voltage sensors and Analog pins 3 | 4 | For instructions, go to https://github.com/ivanseidel/Arduino-Sensors 5 | 6 | Created by Ivan Seidel Gomes, June, 2013. 7 | Released into the public domain. 8 | */ 9 | 10 | #ifndef VoltageInterface_h 11 | #define VoltageInterface_h 12 | 13 | class VoltageInterface 14 | { 15 | public: 16 | // This is the minimum range of the sensor 17 | float minVoltage; 18 | // This is the maximum range of the sensor 19 | float maxVoltage; 20 | 21 | // Should return the CACHED value of the sensor 22 | virtual float getVoltage(); 23 | 24 | // Should READ and CACHE the value of the sensor 25 | virtual float readVoltage(); 26 | }; 27 | 28 | #endif -------------------------------------------------------------------------------- /library.properties: -------------------------------------------------------------------------------- 1 | name=ArduinoSensors 2 | version=1.3 3 | author=Ivan Seidel 4 | maintainer=Ivan Seidel 5 | sentence=A Library of Libraries of Sensors 6 | paragraph=REQUIRES Library. This library was made to provide lot`s of out-of-the-box features, such as 'Interfaces', 'Threadable' sensors, and reliability over C++ Objects. 7 | category=Sensors 8 | url=https://github.com/ivanseidel/ArduinoSensors 9 | architectures=* 10 | depends=ArduinoThread 11 | -------------------------------------------------------------------------------- /sensors/AnalogVoltage.h: -------------------------------------------------------------------------------- 1 | /* 2 | AnalogVoltage.h - A class to help with Analog Input pins. 3 | 4 | This class extends AnalogIn, and is usefull for converting 5 | ADC values to real Voltage values, making it easy to work with 6 | many Arduino Libraries 7 | 8 | https://github.com/ivanseidel/Arduino-Sensors 9 | 10 | Created by Ivan Seidel Gomes, September, 2013. 11 | Released into the public domain. 12 | */ 13 | 14 | #ifndef AnalogVoltage_h 15 | #define AnalogVoltage_h 16 | 17 | #include 18 | 19 | #include 20 | #include 21 | #include 22 | 23 | // Voltage that indicates no voltage assignment 24 | #define NO_VOLTAGE -9999 25 | 26 | class AnalogVoltage: public AnalogIn, public VoltageInterface 27 | { 28 | protected: 29 | // Cached value of the Voltage 30 | float voltage; 31 | 32 | /* 33 | Number of bits used in the conversion 34 | (We need to know this, in order to scale 35 | to the maximum value of the ADC) 36 | 37 | TODO: Currently only working with 10bits AD 38 | */ 39 | // int analogResolution; 40 | // long cachedMaxValue; 41 | static const long cachedMaxValue = 1023; 42 | 43 | /* 44 | This parameter is used for changing the AREF of the arduino. 45 | ARFE is based initialy, by the definition ADC_DEFAULT_AREF, 46 | however you can change it to whatever you want... 47 | */ 48 | static float defaultAnalogReference; 49 | 50 | /* 51 | Convertas an AD value to voltage, based on the maxVoltage 52 | and minVoltage values 53 | */ 54 | virtual float convert(long ADValue){ 55 | float percentage = (((float) ADValue) / cachedMaxValue); 56 | 57 | return (maxVoltage - minVoltage) * percentage + minVoltage; 58 | } 59 | 60 | public: 61 | 62 | /* 63 | Initialize the Input pin, with the desired max and min voltages 64 | 65 | _inPin: Analog input pin to read value 66 | 67 | In order to initialize the object, a maximum voltage must be 68 | assigned, so that the AD conversion can be performed, and a 69 | voltage can be found. 70 | 71 | That voltage usualy is the Arduino Voltage (3.3v, 5v...), so we 72 | can use the definition ADC_DEFAULT_MAX_VOLTAGE to know that voltage. 73 | So: ADC = 0 -> outputs 0.0; 74 | ADC = MAX_ADC_VALUE -> outputs ADC_DEFAULT_AREF; 75 | 76 | However, you can also change the ADC value, or change the AREF pin, 77 | witch would cause a failure on the conversion. 78 | 79 | In case you have changed the AREF, make shure that you initialize 80 | this object with v1 being the maximum voltage, and v2 left unasigned. 81 | So: ADC = 0 -> outputs 0.0; 82 | ADC = MAX_ADC_VALUE -> outputs maxVoltage (v1); 83 | 84 | In other cases, you might be using some external hardware to convert 85 | a battery voltage into a lower voltage, witch can cause the function 86 | to shift up or down (in case of diodes, Amp. Op., ...). 87 | 88 | If that's the case, and you need to add a "base value" to the conversion, 89 | use it as the FIRST parameter, and the maximum value as the SECOND one. 90 | So: ADC = 0 -> outputs minVoltage (v1); 91 | ADC = MAX_ADC_VALUE -> outputs maxVoltage (v2); 92 | */ 93 | AnalogVoltage(int _inPin = -1, float v1 = -9999, float v2 = -9999): AnalogIn(_inPin){ 94 | voltage = 0; 95 | 96 | if(v1 == NO_VOLTAGE && v2 == NO_VOLTAGE){ 97 | // Both voltages are unasigned 98 | minVoltage = 0.0; 99 | maxVoltage = defaultAnalogReference; 100 | }else if(v2 == NO_VOLTAGE){ 101 | minVoltage = 0.0; 102 | maxVoltage = v2; 103 | }else{ 104 | minVoltage = v1; 105 | maxVoltage = v2; 106 | } 107 | } 108 | 109 | /* 110 | Only returns the cached Voltage 111 | */ 112 | virtual float getVoltage(){ 113 | return voltage; 114 | } 115 | 116 | /* 117 | Reads voltage, converts and caches it 118 | */ 119 | virtual float readVoltage(){ 120 | read(); 121 | 122 | voltage = convert(getValue()); 123 | 124 | return getVoltage(); 125 | } 126 | 127 | /* 128 | Called when this Thread is runned 129 | */ 130 | virtual void run(){ 131 | readVoltage(); 132 | 133 | runned(); 134 | } 135 | }; 136 | 137 | // Defines the default analogReference value to the arduino voltage 138 | float AnalogVoltage::defaultAnalogReference = ADC_DEFAULT_AREF * (1 - ADC_VOLTAGE_PROP); 139 | 140 | 141 | #endif -------------------------------------------------------------------------------- /sensors/CompoundDistanceSensor.h: -------------------------------------------------------------------------------- 1 | /* 2 | CompoundDistanceSensor.h - Integrate two Distance Sensor in a single sensor 3 | 4 | This Class helps to integrate two sensors that measure distance with diferent 5 | ranges in a single one, that provides a unique distance. 6 | 7 | For instructions, go to https://github.com/ivanseidel/Arduino-Sensors 8 | 9 | Created by Ivan Seidel Gomes, June, 2013. 10 | Released into the public domain. 11 | */ 12 | #ifndef CompoundDistanceSensor_h 13 | #define CompoundDistanceSensor_h 14 | 15 | #include 16 | #include 17 | 18 | class CompoundDistanceSensor: public Thread, public DistanceInterface 19 | { 20 | protected: 21 | // Cached Last value; 22 | long value; 23 | 24 | DistanceInterface *dist1, *dist2; 25 | 26 | public: 27 | long (*onCalculate)(long, long); 28 | 29 | 30 | // Pin: Analog pin to read value 31 | // lightPin: Pin connected to the LED 32 | CompoundDistanceSensor(DistanceInterface *_dist1, DistanceInterface *_dist2){ 33 | onCalculate = 0; 34 | 35 | // Always set dist1 to have the lowest minimum range 36 | if(_dist1->minDistance < _dist2->minDistance){ 37 | dist1 = _dist1; 38 | dist2 = _dist2; 39 | }else{ 40 | dist1 = _dist2; 41 | dist2 = _dist1; 42 | } 43 | 44 | 45 | minVal = min(dist1->maxVal, dist2->minVal); 46 | maxVal = max(dist1->maxVal, dist2->maxVal); 47 | } 48 | 49 | /* 50 | This method will return the cached value of the calculated distance 51 | */ 52 | virtual float getDistance(){ 53 | return value; 54 | } 55 | 56 | /* 57 | This method will read both distances, and send it to 58 | the callback function 'calculate'. The result will be 59 | saved and returned 60 | */ 61 | virtual float readDistance(){ 62 | if(!dist1 || !dist2) 63 | return -1; 64 | 65 | float val1, val2; 66 | val1 = dist1->readDistance(); 67 | val2 = dist2->readDistance(); 68 | 69 | value = calculate(val1, val2); 70 | 71 | return getDistance(); 72 | } 73 | 74 | virtual float calculate(float val1, float val2){ 75 | if(onCalculate){ 76 | value = onCalculate(val1, val2); 77 | }else{ 78 | // default implementation, if callback not set 79 | if(val1 >= dist1->maxDistance) 80 | value = val2; 81 | else 82 | value = val1; 83 | } 84 | 85 | return value; 86 | } 87 | 88 | /* 89 | Thread implementation 90 | */ 91 | virtual void run(){ 92 | readDistance(); 93 | 94 | runned(); 95 | } 96 | 97 | }; 98 | 99 | #endif -------------------------------------------------------------------------------- /sensors/EZUltrasonicAnalog.h: -------------------------------------------------------------------------------- 1 | /* 2 | HMC6352.h - Honeywell HMC6352 Compass sensor library 3 | 4 | For instructions, go to https://github.com/ivanseidel/Arduino-Sensors 5 | 6 | Datasheet: https://www.sparkfun.com/datasheets/Components/HMC6352.pdf 7 | 8 | Created by Ivan Seidel Gomes, June, 2013. 9 | Released into the public domain. 10 | */ 11 | 12 | #ifndef EZUltrasonic_h 13 | #define EZUltrasonic_h 14 | 15 | #include 16 | #include 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | #include 24 | 25 | /* 26 | Those definitios are used to know the correct 27 | proportional to the VCC value, per cm 28 | */ 29 | #define VCC 1.0 30 | 31 | #define LV_EZ0 toCentimeters(VCC/512) 32 | #define LV_EZ1 toCentimeters(VCC/512) 33 | #define LV_EZ2 toCentimeters(VCC/512) 34 | #define LV_EZ3 toCentimeters(VCC/512) 35 | #define LV_EZ4 toCentimeters(VCC/512) 36 | 37 | #define XL_EZ0 VCC/1024 38 | #define XL_EZ1 VCC/1024 39 | #define XL_EZ2 VCC/1024 40 | #define XL_EZ3 VCC/1024 41 | #define XL_EZ4 VCC/1024 42 | #define XL_EZL0 VCC/1024 43 | 44 | class EZUltrasonicAnalog: public Thread, public DistanceInterface 45 | { 46 | protected: 47 | 48 | // Cached value of the Distance 49 | float distance; 50 | 51 | /* 52 | To prevent Interruptions from reading the distance while it's 53 | being converted, we save it localy and then change the global 54 | */ 55 | float tmpDistance; 56 | 57 | // Object that reads from Analog pin 58 | AnalogIn analogPin; 59 | 60 | // Factor to scale distance 61 | float scaleFactor; 62 | 63 | float convert(float voltage){ 64 | Serial.println(voltage); 65 | // Calculate real Centimiter value 66 | return tmpDistance = voltage * 10;//(voltage / 512) * (scaleFactor * analogPin.maxVoltage); 67 | } 68 | 69 | public: 70 | EZUltrasonicAnalog(int _pin, float _scaleFactor = LV_EZ0){ 71 | analogPin = AnalogIn(_pin); 72 | 73 | distance = 0; 74 | 75 | // TODO: Modify range acordingly to the device selected 76 | minDistance = 10; 77 | maxDistance = 500; 78 | 79 | scaleFactor = _scaleFactor; 80 | 81 | Thread::Thread(); 82 | setInterval(20); 83 | } 84 | 85 | /* 86 | Returns the cached distance 87 | */ 88 | virtual float getDistance(){ 89 | return distance; 90 | } 91 | 92 | /* 93 | Sample ADC, converts to CM and returns cached distance 94 | */ 95 | virtual float readDistance(){ 96 | // Read ADC and converts 97 | // distance = convert(analogPin.readVoltage()); 98 | distance = analogPin.read(); 99 | 100 | return getDistance(); 101 | } 102 | 103 | /* 104 | Thread callback 105 | */ 106 | virtual void run(){ 107 | readDistance(); 108 | 109 | runned(); 110 | } 111 | 112 | }; 113 | 114 | #endif -------------------------------------------------------------------------------- /sensors/HMC6352.h: -------------------------------------------------------------------------------- 1 | /* 2 | HMC6352.h - Honeywell HMC6352 Compass sensor library 3 | 4 | For instructions, go to https://github.com/ivanseidel/Arduino-Sensors 5 | 6 | Datasheet: https://www.sparkfun.com/datasheets/Components/HMC6352.pdf 7 | 8 | Created by Ivan Seidel Gomes, June, 2013. 9 | Released into the public domain. 10 | */ 11 | 12 | #ifndef HMC6352_h 13 | #define HMC6352_h 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | // Default I2C Address 21 | #define HMC6352_ADDR (0x42 >> 1) 22 | 23 | 24 | /* 25 | I2C Commands 26 | */ 27 | #define HMC6352_CMD_WRITE_EEPROM 'w' 28 | #define HMC6352_CMD_READ_EEPROM 'r' 29 | #define HMC6352_CMD_WRITE_RAM 'G' 30 | #define HMC6352_CMD_READ_RAM 'g' 31 | #define HMC6352_CMD_ENTER_SLEEP_MODE 'S' 32 | #define HMC6352_CMD_EXIT_SLEEP_MODE 'W' 33 | #define HMC6352_CMD_UPDATE_BRIDGE_OFFSET 'O' 34 | #define HMC6352_CMD_ENTER_CALIBRATION_MODE 'C' 35 | #define HMC6352_CMD_EXIT_CALIBRATION_MODE 'E' 36 | #define HMC6352_CMD_SAVE_OP_MODE_TO_EEPROM 'L' 37 | #define HMC6352_CMD_GET_DATA 'A' 38 | 39 | /* 40 | EEPROM Addresses 41 | */ 42 | #define HMC6352_EE_SLAVE_ADDRESS 0x00 43 | #define HMC6352_EE_MAGNETOMETER_X_OFFSET_MSB 0x01 44 | #define HMC6352_EE_MAGNETOMETER_X_OFFSET_LSB 0x02 45 | #define HMC6352_EE_MAGNETOMETER_Y_OFFSET_MSB 0x03 46 | #define HMC6352_EE_MAGNETOMETER_Y_OFFSET_LSB 0x04 47 | #define HMC6352_EE_TIME_DELAY 0x05 48 | #define HMC6352_EE_NUMBER_SUMMED_MEASUREMENTS 0x06 49 | #define HMC6352_EE_SOFTWARE_VERSION_NUMBER 0x07 50 | #define HMC6352_EE_OPERATION_MODE_BYTE 0x08 51 | 52 | /* 53 | Operation Mode 54 | */ 55 | 56 | /* 57 | Bits 1 and 0 (Operational Mode Value) 58 | Bit 1 Bit 0 Description 59 | 0 0 Standby Mode 60 | 0 1 Query Mode 61 | 1 0 Continuous Mode 62 | 1 1 Not Allowed 63 | */ 64 | #define HMC6352_MODE_OPERATION_MSK (0b11 << 0) 65 | #define HMC6352_MODE_STANDBY_MODE (0x00 << 0) 66 | #define HMC6352_MODE_QUERY_MODE (0x01 << 0) 67 | #define HMC6352_MODE_CONTINUOUS_MODE (0x02 << 0) 68 | 69 | // Bits 3 and 2 (Always 0) 70 | 71 | /* 72 | Bit 4 (Periodic Set/Reset), 0 = Off, 1 = On 73 | The periodic Set/Reset function performs a re-alignment of the sensors 74 | magnetic domains in case of sensor perming (magnetic upset event), operating temperature shifts, and normal thermal 75 | agitation of the domains. Exposure of the HMC6352 to magnetic fields above 20 gauss (disturbing field threshold) leads to 76 | possible measurement inaccuracy or “stuck” sensor readings until the set/reset function is performed. With the periodic 77 | Set/Reset bit set, the set/reset function occurs every few minutes 78 | */ 79 | #define HMC6352_MODE_PERIODIC_SR_MSK (0x01 << 4) 80 | 81 | /* 82 | Bits 6 and 5 (Continuous Mode Measurement Rate) 83 | Bit 6 Bit 5 Description 84 | 0 0 1 Hz Measurement Rate 85 | 0 1 5 Hz Measurement Rate 86 | 1 0 10 Hz Measurement Rate 87 | 1 1 20 Hz Measurement Rate 88 | */ 89 | #define HMC6352_MODE_RATE_MSK (0b11 << 5) 90 | #define HMC6352_MODE_RATE_1HZ (0x00 << 5) 91 | #define HMC6352_MODE_RATE_5HZ (0x01 << 5) 92 | #define HMC6352_MODE_RATE_10HZ (0x02 << 5) 93 | #define HMC6352_MODE_RATE_20HZ (0x03 << 5) 94 | 95 | // Bit 7 (Always 0) 96 | 97 | 98 | 99 | 100 | class HMC6352: public Thread, public AngleInterface{ 101 | 102 | // Should be protected, but perhaps someone need this stuff... 103 | public: 104 | // Cached value of the angle 105 | float angle; 106 | 107 | // Address of the I2C Compass device 108 | int address; 109 | 110 | // Continuous mode Flag 111 | bool inContinuousMode; 112 | 113 | // Buffer for internal usage 114 | int8_t buffer[4]; 115 | 116 | /* 117 | Sends a single byte command 118 | */ 119 | void sendCommand(int8_t command){ 120 | Wire.beginTransmission(address); 121 | Wire.write(command); 122 | Wire.endTransmission(); 123 | } 124 | 125 | 126 | /* 127 | Reads a single byte after command 128 | */ 129 | int8_t readByte(int8_t command){ 130 | sendCommand(command); 131 | 132 | Wire.requestFrom(address, 1); 133 | while(Wire.available() < 1); 134 | 135 | return Wire.read(); 136 | } 137 | 138 | /* 139 | Writes and reads from the EEPROM 140 | */ 141 | void writeEEPROM(int8_t eeAddress, int8_t data){ 142 | Wire.beginTransmission(address); 143 | Wire.write(HMC6352_CMD_WRITE_EEPROM); 144 | Wire.write(eeAddress); 145 | Wire.write(data); 146 | Wire.endTransmission(); 147 | 148 | delayMicroseconds(70); 149 | } 150 | int8_t readEEPROM(int8_t eeAddress){ 151 | Wire.beginTransmission(address); 152 | Wire.write(HMC6352_CMD_READ_EEPROM); 153 | Wire.write(eeAddress); 154 | Wire.endTransmission(); 155 | 156 | delayMicroseconds(70); 157 | 158 | Wire.requestFrom(address, 1); 159 | while(Wire.available() < 1); 160 | 161 | return Wire.read(); 162 | } 163 | 164 | public: 165 | /* 166 | Instantiate the Compass with the desired address. 167 | if initialize is set to false, then the Device will 168 | only be instantiatet, and no communication will be done 169 | */ 170 | HMC6352(int address = HMC6352_ADDR, bool initialize = true){ 171 | angle = -10; 172 | this->address = address; 173 | 174 | if(initialize){ 175 | Wire.begin(); 176 | 177 | // First readings might be weird.. 178 | readAngle(); 179 | readAngle(); 180 | 181 | // Cache continuousMode flag 182 | inContinuousMode = (getOperationMode() == HMC6352_MODE_CONTINUOUS_MODE); 183 | } 184 | 185 | Thread::Thread(); 186 | setInterval(30); 187 | }; 188 | 189 | virtual float getAngle(){ 190 | return angle; 191 | } 192 | 193 | virtual float readAngle(){ 194 | 195 | // Only sends command if NOT in continuous mode 196 | if(!inContinuousMode) 197 | sendCommand(HMC6352_CMD_GET_DATA); 198 | 199 | Wire.requestFrom(address, 2); 200 | 201 | while(Wire.available() < 2); 202 | 203 | angle = ((Wire.read() << 8) | Wire.read()) / 10.0; 204 | 205 | return angle; 206 | } 207 | 208 | /* 209 | Thread callback 210 | */ 211 | virtual void run(){ 212 | readAngle(); 213 | 214 | runned(); 215 | } 216 | 217 | /* 218 | Get and Set current Mode Byte (RAW) 219 | */ 220 | int8_t getOperationModeRaw(){ 221 | return readEEPROM(HMC6352_EE_OPERATION_MODE_BYTE); 222 | } 223 | void setOperationModeRaw(int8_t val){ 224 | writeEEPROM(HMC6352_EE_OPERATION_MODE_BYTE, val); 225 | delayMicroseconds(120); 226 | } 227 | 228 | /* 229 | Get and Set Sample rate 230 | */ 231 | int8_t getSampleRate(){ 232 | int8_t read = getOperationModeRaw(); 233 | read = ((read >> 5) & 0b11); 234 | 235 | switch(read){ 236 | case 0x00: 237 | return 1; 238 | case 0x01: 239 | return 5; 240 | case 0x02: 241 | return 10; 242 | case 0x03: 243 | return 20; 244 | 245 | default: 246 | return -1; 247 | } 248 | } 249 | void setSampleRate(int rate){ 250 | // Filter rate 251 | int8_t finalRate = 0x00; 252 | 253 | if(rate <= 1) 254 | finalRate = HMC6352_MODE_RATE_1HZ; 255 | else if(rate <= 5) 256 | finalRate = HMC6352_MODE_RATE_5HZ; 257 | else if(rate <= 10) 258 | finalRate = HMC6352_MODE_RATE_10HZ; 259 | else 260 | finalRate = HMC6352_MODE_RATE_20HZ; 261 | 262 | // Get Mode byte 263 | int8_t mode = getOperationModeRaw(); 264 | 265 | mode = (mode & (~HMC6352_MODE_RATE_MSK)) | (finalRate); 266 | 267 | setOperationModeRaw(mode); 268 | } 269 | 270 | /* 271 | Get and Set Opereation mode 272 | */ 273 | int8_t getOperationMode(){ 274 | int8_t read = getOperationModeRaw(); 275 | read = ((read >> 0) & HMC6352_MODE_OPERATION_MSK); 276 | 277 | return read; 278 | } 279 | void setOperationMode(int8_t setMode){ 280 | // Filter rate 281 | int8_t finalMode = setMode & HMC6352_MODE_OPERATION_MSK; 282 | 283 | // Get Mode byte 284 | int8_t mode = getOperationModeRaw(); 285 | 286 | mode = (mode & (~HMC6352_MODE_OPERATION_MSK)) | (finalMode); 287 | 288 | setOperationModeRaw(mode); 289 | 290 | delayMicroseconds(1000); 291 | inContinuousMode = (getOperationMode() == HMC6352_MODE_CONTINUOUS_MODE); 292 | } 293 | 294 | /* 295 | Set the default mode to be Continuous 296 | */ 297 | void setContinuousMode(){ 298 | setOperationMode(HMC6352_MODE_CONTINUOUS_MODE); 299 | } 300 | 301 | /* 302 | Set the default mode to be Standby 303 | */ 304 | void setStandbyMode(){ 305 | setOperationMode(HMC6352_MODE_STANDBY_MODE); 306 | } 307 | 308 | /* 309 | Set the default mode to be Query 310 | */ 311 | void setQueryMode(){ 312 | setOperationMode(HMC6352_MODE_QUERY_MODE); 313 | } 314 | 315 | /* 316 | Periodic Set/Reset mode 317 | */ 318 | bool isAutoFixEnabled(){ 319 | int8_t mode = getOperationModeRaw(); 320 | 321 | return (mode & HMC6352_MODE_PERIODIC_SR_MSK); 322 | } 323 | void setAutoFixEnabled(bool enabled){ 324 | // Get Mode byte 325 | int8_t mode = getOperationModeRaw(); 326 | 327 | mode = (mode & (~HMC6352_MODE_PERIODIC_SR_MSK)) | (enabled * HMC6352_MODE_PERIODIC_SR_MSK); 328 | 329 | setOperationModeRaw(mode); 330 | } 331 | 332 | /* 333 | Enter sleep mode (standby) 334 | */ 335 | void sleep(){ 336 | sendCommand(HMC6352_CMD_ENTER_SLEEP_MODE); 337 | delayMicroseconds(10); 338 | } 339 | 340 | /* 341 | Wake up the device 342 | */ 343 | void wake(){ 344 | sendCommand(HMC6352_CMD_EXIT_SLEEP_MODE); 345 | delayMicroseconds(100); 346 | } 347 | 348 | /* 349 | Save the current mode as default (Sleep or Wake) 350 | */ 351 | void saveMode(){ 352 | sendCommand(HMC6352_CMD_SAVE_OP_MODE_TO_EEPROM); 353 | } 354 | 355 | /* 356 | Calibration mode. 357 | 358 | Recommended time: 6 seconds up to 3 minutes. 359 | Optimal: 2 rotations in 20 seconds. 360 | (Remember to FINISH calibration) 361 | */ 362 | void startCalibration(){ 363 | sendCommand(HMC6352_CMD_ENTER_CALIBRATION_MODE); 364 | } 365 | void finishCalibration(){ 366 | sendCommand(HMC6352_CMD_EXIT_CALIBRATION_MODE); 367 | } 368 | 369 | /* 370 | Facilitate calibration. 371 | call this method with the desired duration (in Ms) 372 | and the Compass will enter and exit both states after 373 | the desired duration. 374 | */ 375 | void calibrate(long durationMs = 20000){ 376 | startCalibration(); 377 | 378 | // We use Microseconds to allow calibration on Interruptions 379 | delayMicroseconds(durationMs*1000); 380 | 381 | finishCalibration(); 382 | } 383 | 384 | /* 385 | Get version 386 | */ 387 | int8_t getVersion(){ 388 | return readEEPROM(HMC6352_EE_SOFTWARE_VERSION_NUMBER); 389 | } 390 | 391 | 392 | /* 393 | Get and Set address 394 | */ 395 | int8_t getAddress(){ 396 | return address << 1; 397 | } 398 | 399 | void setAddress(int8_t newAddress){ 400 | writeEEPROM(HMC6352_EE_SLAVE_ADDRESS, newAddress); 401 | 402 | // Also change the local address (otherwise we will lost the device...) 403 | address = (newAddress >> 1); 404 | 405 | delayMicroseconds(300); 406 | } 407 | 408 | /* 409 | ========== SANDBOX ========== 410 | In development: 411 | Search devices reliably 412 | */ 413 | 414 | /* 415 | This will return the same address of the local variable, 416 | however is usefull for checking if the device exist over 417 | the I2C wiring. 418 | 419 | timeoutMs is the maximum time waiting for a response. 420 | */ 421 | /*int8_t getAddressDumb(long timeoutMs){ 422 | Wire.beginTransmission(address); 423 | Wire.write(HMC6352_CMD_READ_EEPROM); 424 | Wire.write(HMC6352_EE_SOFTWARE_VERSION_NUMBER); 425 | Wire.endTransmission(); 426 | 427 | delayMicroseconds(70); 428 | 429 | Wire.requestFrom(address, 1); 430 | 431 | long start = micros(); 432 | while(Wire.available() < 1) 433 | if(micros() - start > timeoutMs*1000) 434 | return 0; 435 | 436 | return Wire.read(); 437 | }*/ 438 | 439 | /* 440 | This method will return the first HMC6352 over the I2C bus, 441 | with the address closest and greater than startAddress. 442 | 443 | If none is found, false is returned; 444 | */ 445 | 446 | /*static int findDevice(int startAddress = 1){ 447 | HMC6352 searchDevice(startAddress, false); 448 | 449 | for(int8_t i = startAddress; i < 0xFF; i++){ 450 | searchDevice.address = (i >> 1); 451 | 452 | Serial.print(i); 453 | Serial.print("\t"); 454 | int8_t foundAdr = searchDevice.getAddressDumb(1); 455 | Serial.println(foundAdr); 456 | if(foundAdr == i){ 457 | Serial.print("Found: "); 458 | Serial.println(i); 459 | return i; 460 | } 461 | } 462 | 463 | Serial.println("NOT FOUND"); 464 | 465 | return false; 466 | }*/ 467 | 468 | }; 469 | 470 | #endif -------------------------------------------------------------------------------- /sensors/MLX90614.h: -------------------------------------------------------------------------------- 1 | /* 2 | MLX90614.h - Reliable Library that controlls MLX90614 Temperature sensors 3 | 4 | For instructions, go to https://github.com/ivanseidel/Arduino-Sensors 5 | 6 | Created by Ivan Seidel Gomes, June, 2013. 7 | Released into the public domain. 8 | */ 9 | 10 | #ifndef MLX90614_h 11 | #define MLX90614_h 12 | 13 | #include 14 | 15 | #include 16 | #include 17 | 18 | #include 19 | #include 20 | 21 | // I2C bits to read and write 22 | #define I2C_READ 0x1 23 | #define I2C_WRITE 0x0 24 | 25 | /* 26 | MLX90614 Memory addresses 27 | */ 28 | #define MLX90614_ADR_SET_ADDRESS 0x2E 29 | #define MLX90614_ADR_GET_TEMP1 0x07 30 | 31 | /* 32 | Table used to do CRC8 Algorithm, to change Address 33 | */ 34 | const unsigned char crc_table[] = { 35 | 0x00,0x07,0x0E,0x09,0x1C,0x1B,0x12,0x15,0x38,0x3F,0x36,0x31, 36 | 0x24,0x23,0x2A,0x2D,0x70,0x77,0x7E,0x79,0x6C,0x6B,0x62,0x65, 37 | 0x48,0x4F,0x46,0x41,0x54,0x53,0x5A,0x5D,0xE0,0xE7,0xEE,0xE9, 38 | 0xFC,0xFB,0xF2,0xF5,0xD8,0xDF,0xD6,0xD1,0xC4,0xC3,0xCA,0xCD, 39 | 0x90,0x97,0x9E,0x99,0x8C,0x8B,0x82,0x85,0xA8,0xAF,0xA6,0xA1, 40 | 0xB4,0xB3,0xBA,0xBD,0xC7,0xC0,0xC9,0xCE,0xDB,0xDC,0xD5,0xD2, 41 | 0xFF,0xF8,0xF1,0xF6,0xE3,0xE4,0xED,0xEA,0xB7,0xB0,0xB9,0xBE, 42 | 0xAB,0xAC,0xA5,0xA2,0x8F,0x88,0x81,0x86,0x93,0x94,0x9D,0x9A, 43 | 0x27,0x20,0x29,0x2E,0x3B,0x3C,0x35,0x32,0x1F,0x18,0x11,0x16, 44 | 0x03,0x04,0x0D,0x0A,0x57,0x50,0x59,0x5E,0x4B,0x4C,0x45,0x42, 45 | 0x6F,0x68,0x61,0x66,0x73,0x74,0x7D,0x7A,0x89,0x8E,0x87,0x80, 46 | 0x95,0x92,0x9B,0x9C,0xB1,0xB6,0xBF,0xB8,0xAD,0xAA,0xA3,0xA4, 47 | 0xF9,0xFE,0xF7,0xF0,0xE5,0xE2,0xEB,0xEC,0xC1,0xC6,0xCF,0xC8, 48 | 0xDD,0xDA,0xD3,0xD4,0x69,0x6E,0x67,0x60,0x75,0x72,0x7B,0x7C, 49 | 0x51,0x56,0x5F,0x58,0x4D,0x4A,0x43,0x44,0x19,0x1E,0x17,0x10, 50 | 0x05,0x02,0x0B,0x0C,0x21,0x26,0x2F,0x28,0x3D,0x3A,0x33,0x34, 51 | 0x4E,0x49,0x40,0x47,0x52,0x55,0x5C,0x5B,0x76,0x71,0x78,0x7F, 52 | 0x6A,0x6D,0x64,0x63,0x3E,0x39,0x30,0x37,0x22,0x25,0x2C,0x2B, 53 | 0x06,0x01,0x08,0x0F,0x1A,0x1D,0x14,0x13,0xAE,0xA9,0xA0,0xA7, 54 | 0xB2,0xB5,0xBC,0xBB,0x96,0x91,0x98,0x9F,0x8A,0x8D,0x84,0x83, 55 | 0xDE,0xD9,0xD0,0xD7,0xC2,0xC5,0xCC,0xCB,0xE6,0xE1,0xE8,0xEF, 56 | 0xFA,0xFD,0xF4,0xF3 57 | }; 58 | 59 | class MLX90614: public Thread, public TemperatureInterface{ 60 | protected: 61 | /* 62 | This method is used when changing the address of the MLX90614 63 | */ 64 | static char CRC8(unsigned char buffer[], unsigned int len){ 65 | unsigned char m_crc = 0; 66 | int m_byte_count = 0; 67 | 68 | for(int i = 0; i < len; i++) { 69 | m_byte_count++; 70 | m_crc = crc_table[ m_crc ^ buffer[i] ]; 71 | } 72 | 73 | return m_crc; 74 | } 75 | 76 | /* 77 | Keeps the current address of the MLX90614 78 | */ 79 | int adr; 80 | 81 | /* 82 | Cached value of the temperature 83 | */ 84 | float temperature; 85 | 86 | // Flag that indicates if the bus is initialized 87 | static bool busInitialized; 88 | 89 | public: 90 | /* 91 | Construct the MLX90614 object with the given address 92 | */ 93 | MLX90614(int _adr){ 94 | setAddress(_adr); 95 | 96 | if(!MLX90614::busInitialized){ 97 | MLX90614::init(); 98 | MLX90614::busInitialized = true; 99 | } 100 | 101 | Thread::Thread(); 102 | setInterval(20); 103 | } 104 | 105 | /* 106 | Initialize I2C bus and enable pull-ups 107 | */ 108 | static void init(){ 109 | // Wire.begin(); 110 | i2c_init(); //Initialise the i2c bus 111 | PORTC = (1 << PORTC4) | (1 << PORTC5);//enable pullups 112 | } 113 | 114 | /* 115 | Set the current object address to the given one. 116 | 117 | NOTE: It doesen't actually change the device Address. 118 | See 'changeAddress' for that purpose 119 | */ 120 | void setAddress(int _adr){ 121 | adr = _adr; 122 | } 123 | 124 | /* 125 | Return the current device address 126 | */ 127 | int getAddress(){ 128 | return adr; 129 | } 130 | 131 | /* 132 | Put the desired device to sleep 133 | */ 134 | bool sleep(){ 135 | if(!i2c_start_wait(adr<<1 + I2C_WRITE)) 136 | return false; 137 | 138 | i2c_write(0xFF); 139 | i2c_write(0xE8); 140 | i2c_stop(); 141 | 142 | return true; 143 | } 144 | 145 | /* 146 | Wake ALL MLX90614 devices on the I2C bus 147 | */ 148 | void wake(){ 149 | digitalWrite(SDA, LOW); 150 | delayMicroseconds(36000); 151 | digitalWrite(SDA, HIGH); 152 | } 153 | 154 | /* 155 | Comunicate with the sensor, sample the Temperature 156 | and save to the Cached value 157 | */ 158 | virtual float readTemperature(){ 159 | 160 | int8_t _adr = adr << 1; 161 | int data_low = 0; 162 | int data_high = 0; 163 | int pec = 0; 164 | 165 | i2c_start_wait(_adr | I2C_WRITE); 166 | i2c_write(MLX90614_ADR_GET_TEMP1); 167 | 168 | // read 169 | i2c_rep_start(_adr | I2C_READ); 170 | data_low = i2c_readAck(); //Read 1 byte and then send ack 171 | data_high = i2c_readAck(); //Read 1 byte and then send ack 172 | pec = i2c_readNak(); 173 | i2c_stop(); 174 | 175 | /* 176 | This converts high and low bytes together and processes temperature, 177 | MSB is a error bit and is ignored for temps 178 | */ 179 | // 0.02 degrees per LSB (measurement resolution of the MLX90614) 180 | double tempFactor = 0.02; 181 | 182 | // Zero the data 183 | double tempData = 0x0000; 184 | 185 | // Data past the decimal point 186 | int frac; 187 | 188 | /* 189 | This masks off the error bit of the high byte, 190 | then moves it left 8 bits and adds the low byte. 191 | */ 192 | 193 | tempData = (double)(((data_high & 0x007F) << 8) + data_low); 194 | tempData = (tempData * tempFactor)-0.01; 195 | 196 | float celcius = tempData - 273.15; 197 | 198 | /* 199 | If you are using ATMEGA328 or like processors, 200 | the following line end's up with I2C failures. 201 | */ 202 | TWCR = 0; 203 | 204 | temperature = celcius; 205 | 206 | return getTemperature(); 207 | } 208 | 209 | /* 210 | Returns the cached value of the temperature 211 | */ 212 | virtual float getTemperature(){ 213 | return temperature; 214 | } 215 | 216 | /* 217 | Thread callback 218 | */ 219 | virtual void run(){ 220 | readTemperature(); 221 | 222 | runned(); 223 | } 224 | 225 | /* 226 | Change the address of the MLX90614 over the I2C. 227 | 228 | Procedure: 229 | 1.: Remove ALL devices from the BUS and let only the 230 | desired MLX90614 to change address 231 | 232 | 2.: Write a code that calls changeAddress(addr) 233 | once and stops. 234 | 235 | 3.: Power off and on the MLX90614. 236 | 237 | 4.: Done. Have fun! 238 | */ 239 | bool changeAddress(int newAdr){ 240 | // Erase Address memory FIRST 241 | if(newAdr != 0x00) 242 | changeAddress(0x00); 243 | 244 | // Prepare 3 byte message 245 | byte msg[4] = {MLX90614_ADR_SET_ADDRESS, newAdr, 0x00,0x00}; 246 | msg[3] = CRC8(msg, 3); 247 | 248 | if(!i2c_start_wait(0 + I2C_WRITE)) 249 | return false; 250 | 251 | i2c_write(msg[0]); 252 | i2c_write(msg[1]); 253 | i2c_write(msg[2]); 254 | 255 | if(i2c_write(msg[3])){ 256 | i2c_stop(); 257 | }else{ 258 | i2c_stop(); 259 | return false; 260 | } 261 | 262 | if(newAdr != 0x00) 263 | sleep(); 264 | 265 | // DelayMicroseconds can be used within Interruptions 266 | delayMicroseconds(50000); 267 | 268 | if(newAdr != 0x00) 269 | wake(); 270 | 271 | adr = newAdr; 272 | 273 | return true; 274 | } 275 | 276 | /* 277 | This method will return the first connected and working device 278 | address between 'start' and 'end' 279 | */ 280 | static int scan(int start = 0, int end = 127){ 281 | bool find; 282 | int i; 283 | 284 | for(i = start; i <= end; i++){ 285 | find = i2c_start_wait(i<<1 | I2C_WRITE); 286 | i2c_stop(); 287 | 288 | if(find) 289 | return i; 290 | 291 | TWCR = 0; 292 | } 293 | return -1; 294 | } 295 | 296 | /* 297 | This method will return a list of connected MLX90614 addresses 298 | connected and working, between 'start' and 'end', with a limit 299 | of 'maxSize' devices found. All devices will be stored in the 300 | 'addresses' buffer (IT SHOULD STORE AT LEAST 'maxSize' int elements). 301 | 302 | The total number of devices found will be returned. 303 | */ 304 | static int scanAll(int start, int end, int maxSize, int* addresses){ 305 | int total; 306 | int s; 307 | total = 0; 308 | for(int x = start; x < end && total < maxSize; x++){ 309 | s = scan(x); 310 | if(s==-1){ 311 | break; 312 | }else{ 313 | addresses[total] = s; 314 | x = s; 315 | total++; 316 | } 317 | } 318 | return total; 319 | } 320 | }; 321 | 322 | // Static atributes initialization 323 | bool MLX90614::busInitialized = false; 324 | 325 | #endif 326 | -------------------------------------------------------------------------------- /sensors/MPU/MPU9150.h: -------------------------------------------------------------------------------- 1 | // I2Cdev library collection - MPU9150 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 _MPU9150_H_ 38 | #define _MPU9150_H_ 39 | 40 | #include 41 | #include "sensors/MPU/helper_3dmath.h" 42 | 43 | //Magnetometer Registers 44 | #define MPU9150_RA_MAG_ADDRESS 0x0C 45 | #define MPU9150_RA_MAG_XOUT_L 0x03 46 | #define MPU9150_RA_MAG_XOUT_H 0x04 47 | #define MPU9150_RA_MAG_YOUT_L 0x05 48 | #define MPU9150_RA_MAG_YOUT_H 0x06 49 | #define MPU9150_RA_MAG_ZOUT_L 0x07 50 | #define MPU9150_RA_MAG_ZOUT_H 0x08 51 | 52 | #define MPU9150_ADDRESS_AD0_LOW 0x68 // address pin low (GND), default for InvenSense evaluation board 53 | #define MPU9150_ADDRESS_AD0_HIGH 0x69 // address pin high (VCC) 54 | #define MPU9150_DEFAULT_ADDRESS MPU9150_ADDRESS_AD0_LOW 55 | 56 | #define MPU9150_RA_XG_OFFS_TC 0x00 //[7] PWR_MODE, [6:1] XG_OFFS_TC, [0] OTP_BNK_VLD 57 | #define MPU9150_RA_YG_OFFS_TC 0x01 //[7] PWR_MODE, [6:1] YG_OFFS_TC, [0] OTP_BNK_VLD 58 | #define MPU9150_RA_ZG_OFFS_TC 0x02 //[7] PWR_MODE, [6:1] ZG_OFFS_TC, [0] OTP_BNK_VLD 59 | #define MPU9150_RA_X_FINE_GAIN 0x03 //[7:0] X_FINE_GAIN 60 | #define MPU9150_RA_Y_FINE_GAIN 0x04 //[7:0] Y_FINE_GAIN 61 | #define MPU9150_RA_Z_FINE_GAIN 0x05 //[7:0] Z_FINE_GAIN 62 | #define MPU9150_RA_XA_OFFS_H 0x06 //[15:0] XA_OFFS 63 | #define MPU9150_RA_XA_OFFS_L_TC 0x07 64 | #define MPU9150_RA_YA_OFFS_H 0x08 //[15:0] YA_OFFS 65 | #define MPU9150_RA_YA_OFFS_L_TC 0x09 66 | #define MPU9150_RA_ZA_OFFS_H 0x0A //[15:0] ZA_OFFS 67 | #define MPU9150_RA_ZA_OFFS_L_TC 0x0B 68 | #define MPU9150_RA_XG_OFFS_USRH 0x13 //[15:0] XG_OFFS_USR 69 | #define MPU9150_RA_XG_OFFS_USRL 0x14 70 | #define MPU9150_RA_YG_OFFS_USRH 0x15 //[15:0] YG_OFFS_USR 71 | #define MPU9150_RA_YG_OFFS_USRL 0x16 72 | #define MPU9150_RA_ZG_OFFS_USRH 0x17 //[15:0] ZG_OFFS_USR 73 | #define MPU9150_RA_ZG_OFFS_USRL 0x18 74 | #define MPU9150_RA_SMPLRT_DIV 0x19 75 | #define MPU9150_RA_CONFIG 0x1A 76 | #define MPU9150_RA_GYRO_CONFIG 0x1B 77 | #define MPU9150_RA_ACCEL_CONFIG 0x1C 78 | #define MPU9150_RA_FF_THR 0x1D 79 | #define MPU9150_RA_FF_DUR 0x1E 80 | #define MPU9150_RA_MOT_THR 0x1F 81 | #define MPU9150_RA_MOT_DUR 0x20 82 | #define MPU9150_RA_ZRMOT_THR 0x21 83 | #define MPU9150_RA_ZRMOT_DUR 0x22 84 | #define MPU9150_RA_FIFO_EN 0x23 85 | #define MPU9150_RA_I2C_MST_CTRL 0x24 86 | #define MPU9150_RA_I2C_SLV0_ADDR 0x25 87 | #define MPU9150_RA_I2C_SLV0_REG 0x26 88 | #define MPU9150_RA_I2C_SLV0_CTRL 0x27 89 | #define MPU9150_RA_I2C_SLV1_ADDR 0x28 90 | #define MPU9150_RA_I2C_SLV1_REG 0x29 91 | #define MPU9150_RA_I2C_SLV1_CTRL 0x2A 92 | #define MPU9150_RA_I2C_SLV2_ADDR 0x2B 93 | #define MPU9150_RA_I2C_SLV2_REG 0x2C 94 | #define MPU9150_RA_I2C_SLV2_CTRL 0x2D 95 | #define MPU9150_RA_I2C_SLV3_ADDR 0x2E 96 | #define MPU9150_RA_I2C_SLV3_REG 0x2F 97 | #define MPU9150_RA_I2C_SLV3_CTRL 0x30 98 | #define MPU9150_RA_I2C_SLV4_ADDR 0x31 99 | #define MPU9150_RA_I2C_SLV4_REG 0x32 100 | #define MPU9150_RA_I2C_SLV4_DO 0x33 101 | #define MPU9150_RA_I2C_SLV4_CTRL 0x34 102 | #define MPU9150_RA_I2C_SLV4_DI 0x35 103 | #define MPU9150_RA_I2C_MST_STATUS 0x36 104 | #define MPU9150_RA_INT_PIN_CFG 0x37 105 | #define MPU9150_RA_INT_ENABLE 0x38 106 | #define MPU9150_RA_DMP_INT_STATUS 0x39 107 | #define MPU9150_RA_INT_STATUS 0x3A 108 | #define MPU9150_RA_ACCEL_XOUT_H 0x3B 109 | #define MPU9150_RA_ACCEL_XOUT_L 0x3C 110 | #define MPU9150_RA_ACCEL_YOUT_H 0x3D 111 | #define MPU9150_RA_ACCEL_YOUT_L 0x3E 112 | #define MPU9150_RA_ACCEL_ZOUT_H 0x3F 113 | #define MPU9150_RA_ACCEL_ZOUT_L 0x40 114 | #define MPU9150_RA_TEMP_OUT_H 0x41 115 | #define MPU9150_RA_TEMP_OUT_L 0x42 116 | #define MPU9150_RA_GYRO_XOUT_H 0x43 117 | #define MPU9150_RA_GYRO_XOUT_L 0x44 118 | #define MPU9150_RA_GYRO_YOUT_H 0x45 119 | #define MPU9150_RA_GYRO_YOUT_L 0x46 120 | #define MPU9150_RA_GYRO_ZOUT_H 0x47 121 | #define MPU9150_RA_GYRO_ZOUT_L 0x48 122 | #define MPU9150_RA_EXT_SENS_DATA_00 0x49 123 | #define MPU9150_RA_EXT_SENS_DATA_01 0x4A 124 | #define MPU9150_RA_EXT_SENS_DATA_02 0x4B 125 | #define MPU9150_RA_EXT_SENS_DATA_03 0x4C 126 | #define MPU9150_RA_EXT_SENS_DATA_04 0x4D 127 | #define MPU9150_RA_EXT_SENS_DATA_05 0x4E 128 | #define MPU9150_RA_EXT_SENS_DATA_06 0x4F 129 | #define MPU9150_RA_EXT_SENS_DATA_07 0x50 130 | #define MPU9150_RA_EXT_SENS_DATA_08 0x51 131 | #define MPU9150_RA_EXT_SENS_DATA_09 0x52 132 | #define MPU9150_RA_EXT_SENS_DATA_10 0x53 133 | #define MPU9150_RA_EXT_SENS_DATA_11 0x54 134 | #define MPU9150_RA_EXT_SENS_DATA_12 0x55 135 | #define MPU9150_RA_EXT_SENS_DATA_13 0x56 136 | #define MPU9150_RA_EXT_SENS_DATA_14 0x57 137 | #define MPU9150_RA_EXT_SENS_DATA_15 0x58 138 | #define MPU9150_RA_EXT_SENS_DATA_16 0x59 139 | #define MPU9150_RA_EXT_SENS_DATA_17 0x5A 140 | #define MPU9150_RA_EXT_SENS_DATA_18 0x5B 141 | #define MPU9150_RA_EXT_SENS_DATA_19 0x5C 142 | #define MPU9150_RA_EXT_SENS_DATA_20 0x5D 143 | #define MPU9150_RA_EXT_SENS_DATA_21 0x5E 144 | #define MPU9150_RA_EXT_SENS_DATA_22 0x5F 145 | #define MPU9150_RA_EXT_SENS_DATA_23 0x60 146 | #define MPU9150_RA_MOT_DETECT_STATUS 0x61 147 | #define MPU9150_RA_I2C_SLV0_DO 0x63 148 | #define MPU9150_RA_I2C_SLV1_DO 0x64 149 | #define MPU9150_RA_I2C_SLV2_DO 0x65 150 | #define MPU9150_RA_I2C_SLV3_DO 0x66 151 | #define MPU9150_RA_I2C_MST_DELAY_CTRL 0x67 152 | #define MPU9150_RA_SIGNAL_PATH_RESET 0x68 153 | #define MPU9150_RA_MOT_DETECT_CTRL 0x69 154 | #define MPU9150_RA_USER_CTRL 0x6A 155 | #define MPU9150_RA_PWR_MGMT_1 0x6B 156 | #define MPU9150_RA_PWR_MGMT_2 0x6C 157 | #define MPU9150_RA_BANK_SEL 0x6D 158 | #define MPU9150_RA_MEM_START_ADDR 0x6E 159 | #define MPU9150_RA_MEM_R_W 0x6F 160 | #define MPU9150_RA_DMP_CFG_1 0x70 161 | #define MPU9150_RA_DMP_CFG_2 0x71 162 | #define MPU9150_RA_FIFO_COUNTH 0x72 163 | #define MPU9150_RA_FIFO_COUNTL 0x73 164 | #define MPU9150_RA_FIFO_R_W 0x74 165 | #define MPU9150_RA_WHO_AM_I 0x75 166 | 167 | #define MPU9150_TC_PWR_MODE_BIT 7 168 | #define MPU9150_TC_OFFSET_BIT 6 169 | #define MPU9150_TC_OFFSET_LENGTH 6 170 | #define MPU9150_TC_OTP_BNK_VLD_BIT 0 171 | 172 | #define MPU9150_VDDIO_LEVEL_VLOGIC 0 173 | #define MPU9150_VDDIO_LEVEL_VDD 1 174 | 175 | #define MPU9150_CFG_EXT_SYNC_SET_BIT 5 176 | #define MPU9150_CFG_EXT_SYNC_SET_LENGTH 3 177 | #define MPU9150_CFG_DLPF_CFG_BIT 2 178 | #define MPU9150_CFG_DLPF_CFG_LENGTH 3 179 | 180 | #define MPU9150_EXT_SYNC_DISABLED 0x0 181 | #define MPU9150_EXT_SYNC_TEMP_OUT_L 0x1 182 | #define MPU9150_EXT_SYNC_GYRO_XOUT_L 0x2 183 | #define MPU9150_EXT_SYNC_GYRO_YOUT_L 0x3 184 | #define MPU9150_EXT_SYNC_GYRO_ZOUT_L 0x4 185 | #define MPU9150_EXT_SYNC_ACCEL_XOUT_L 0x5 186 | #define MPU9150_EXT_SYNC_ACCEL_YOUT_L 0x6 187 | #define MPU9150_EXT_SYNC_ACCEL_ZOUT_L 0x7 188 | 189 | #define MPU9150_DLPF_BW_256 0x00 190 | #define MPU9150_DLPF_BW_188 0x01 191 | #define MPU9150_DLPF_BW_98 0x02 192 | #define MPU9150_DLPF_BW_42 0x03 193 | #define MPU9150_DLPF_BW_20 0x04 194 | #define MPU9150_DLPF_BW_10 0x05 195 | #define MPU9150_DLPF_BW_5 0x06 196 | 197 | #define MPU9150_GCONFIG_FS_SEL_BIT 4 198 | #define MPU9150_GCONFIG_FS_SEL_LENGTH 2 199 | 200 | #define MPU9150_GYRO_FS_250 0x00 201 | #define MPU9150_GYRO_FS_500 0x01 202 | #define MPU9150_GYRO_FS_1000 0x02 203 | #define MPU9150_GYRO_FS_2000 0x03 204 | 205 | #define MPU9150_ACONFIG_XA_ST_BIT 7 206 | #define MPU9150_ACONFIG_YA_ST_BIT 6 207 | #define MPU9150_ACONFIG_ZA_ST_BIT 5 208 | #define MPU9150_ACONFIG_AFS_SEL_BIT 4 209 | #define MPU9150_ACONFIG_AFS_SEL_LENGTH 2 210 | #define MPU9150_ACONFIG_ACCEL_HPF_BIT 2 211 | #define MPU9150_ACONFIG_ACCEL_HPF_LENGTH 3 212 | 213 | #define MPU9150_ACCEL_FS_2 0x00 214 | #define MPU9150_ACCEL_FS_4 0x01 215 | #define MPU9150_ACCEL_FS_8 0x02 216 | #define MPU9150_ACCEL_FS_16 0x03 217 | 218 | #define MPU9150_DHPF_RESET 0x00 219 | #define MPU9150_DHPF_5 0x01 220 | #define MPU9150_DHPF_2P5 0x02 221 | #define MPU9150_DHPF_1P25 0x03 222 | #define MPU9150_DHPF_0P63 0x04 223 | #define MPU9150_DHPF_HOLD 0x07 224 | 225 | #define MPU9150_TEMP_FIFO_EN_BIT 7 226 | #define MPU9150_XG_FIFO_EN_BIT 6 227 | #define MPU9150_YG_FIFO_EN_BIT 5 228 | #define MPU9150_ZG_FIFO_EN_BIT 4 229 | #define MPU9150_ACCEL_FIFO_EN_BIT 3 230 | #define MPU9150_SLV2_FIFO_EN_BIT 2 231 | #define MPU9150_SLV1_FIFO_EN_BIT 1 232 | #define MPU9150_SLV0_FIFO_EN_BIT 0 233 | 234 | #define MPU9150_MULT_MST_EN_BIT 7 235 | #define MPU9150_WAIT_FOR_ES_BIT 6 236 | #define MPU9150_SLV_3_FIFO_EN_BIT 5 237 | #define MPU9150_I2C_MST_P_NSR_BIT 4 238 | #define MPU9150_I2C_MST_CLK_BIT 3 239 | #define MPU9150_I2C_MST_CLK_LENGTH 4 240 | 241 | #define MPU9150_CLOCK_DIV_348 0x0 242 | #define MPU9150_CLOCK_DIV_333 0x1 243 | #define MPU9150_CLOCK_DIV_320 0x2 244 | #define MPU9150_CLOCK_DIV_308 0x3 245 | #define MPU9150_CLOCK_DIV_296 0x4 246 | #define MPU9150_CLOCK_DIV_286 0x5 247 | #define MPU9150_CLOCK_DIV_276 0x6 248 | #define MPU9150_CLOCK_DIV_267 0x7 249 | #define MPU9150_CLOCK_DIV_258 0x8 250 | #define MPU9150_CLOCK_DIV_500 0x9 251 | #define MPU9150_CLOCK_DIV_471 0xA 252 | #define MPU9150_CLOCK_DIV_444 0xB 253 | #define MPU9150_CLOCK_DIV_421 0xC 254 | #define MPU9150_CLOCK_DIV_400 0xD 255 | #define MPU9150_CLOCK_DIV_381 0xE 256 | #define MPU9150_CLOCK_DIV_364 0xF 257 | 258 | #define MPU9150_I2C_SLV_RW_BIT 7 259 | #define MPU9150_I2C_SLV_ADDR_BIT 6 260 | #define MPU9150_I2C_SLV_ADDR_LENGTH 7 261 | #define MPU9150_I2C_SLV_EN_BIT 7 262 | #define MPU9150_I2C_SLV_BYTE_SW_BIT 6 263 | #define MPU9150_I2C_SLV_REG_DIS_BIT 5 264 | #define MPU9150_I2C_SLV_GRP_BIT 4 265 | #define MPU9150_I2C_SLV_LEN_BIT 3 266 | #define MPU9150_I2C_SLV_LEN_LENGTH 4 267 | 268 | #define MPU9150_I2C_SLV4_RW_BIT 7 269 | #define MPU9150_I2C_SLV4_ADDR_BIT 6 270 | #define MPU9150_I2C_SLV4_ADDR_LENGTH 7 271 | #define MPU9150_I2C_SLV4_EN_BIT 7 272 | #define MPU9150_I2C_SLV4_INT_EN_BIT 6 273 | #define MPU9150_I2C_SLV4_REG_DIS_BIT 5 274 | #define MPU9150_I2C_SLV4_MST_DLY_BIT 4 275 | #define MPU9150_I2C_SLV4_MST_DLY_LENGTH 5 276 | 277 | #define MPU9150_MST_PASS_THROUGH_BIT 7 278 | #define MPU9150_MST_I2C_SLV4_DONE_BIT 6 279 | #define MPU9150_MST_I2C_LOST_ARB_BIT 5 280 | #define MPU9150_MST_I2C_SLV4_NACK_BIT 4 281 | #define MPU9150_MST_I2C_SLV3_NACK_BIT 3 282 | #define MPU9150_MST_I2C_SLV2_NACK_BIT 2 283 | #define MPU9150_MST_I2C_SLV1_NACK_BIT 1 284 | #define MPU9150_MST_I2C_SLV0_NACK_BIT 0 285 | 286 | #define MPU9150_INTCFG_INT_LEVEL_BIT 7 287 | #define MPU9150_INTCFG_INT_OPEN_BIT 6 288 | #define MPU9150_INTCFG_LATCH_INT_EN_BIT 5 289 | #define MPU9150_INTCFG_INT_RD_CLEAR_BIT 4 290 | #define MPU9150_INTCFG_FSYNC_INT_LEVEL_BIT 3 291 | #define MPU9150_INTCFG_FSYNC_INT_EN_BIT 2 292 | #define MPU9150_INTCFG_I2C_BYPASS_EN_BIT 1 293 | #define MPU9150_INTCFG_CLKOUT_EN_BIT 0 294 | 295 | #define MPU9150_INTMODE_ACTIVEHIGH 0x00 296 | #define MPU9150_INTMODE_ACTIVELOW 0x01 297 | 298 | #define MPU9150_INTDRV_PUSHPULL 0x00 299 | #define MPU9150_INTDRV_OPENDRAIN 0x01 300 | 301 | #define MPU9150_INTLATCH_50USPULSE 0x00 302 | #define MPU9150_INTLATCH_WAITCLEAR 0x01 303 | 304 | #define MPU9150_INTCLEAR_STATUSREAD 0x00 305 | #define MPU9150_INTCLEAR_ANYREAD 0x01 306 | 307 | #define MPU9150_INTERRUPT_FF_BIT 7 308 | #define MPU9150_INTERRUPT_MOT_BIT 6 309 | #define MPU9150_INTERRUPT_ZMOT_BIT 5 310 | #define MPU9150_INTERRUPT_FIFO_OFLOW_BIT 4 311 | #define MPU9150_INTERRUPT_I2C_MST_INT_BIT 3 312 | #define MPU9150_INTERRUPT_PLL_RDY_INT_BIT 2 313 | #define MPU9150_INTERRUPT_DMP_INT_BIT 1 314 | #define MPU9150_INTERRUPT_DATA_RDY_BIT 0 315 | 316 | // TODO: figure out what these actually do 317 | // UMPL source code is not very obivous 318 | #define MPU9150_DMPINT_5_BIT 5 319 | #define MPU9150_DMPINT_4_BIT 4 320 | #define MPU9150_DMPINT_3_BIT 3 321 | #define MPU9150_DMPINT_2_BIT 2 322 | #define MPU9150_DMPINT_1_BIT 1 323 | #define MPU9150_DMPINT_0_BIT 0 324 | 325 | #define MPU9150_MOTION_MOT_XNEG_BIT 7 326 | #define MPU9150_MOTION_MOT_XPOS_BIT 6 327 | #define MPU9150_MOTION_MOT_YNEG_BIT 5 328 | #define MPU9150_MOTION_MOT_YPOS_BIT 4 329 | #define MPU9150_MOTION_MOT_ZNEG_BIT 3 330 | #define MPU9150_MOTION_MOT_ZPOS_BIT 2 331 | #define MPU9150_MOTION_MOT_ZRMOT_BIT 0 332 | 333 | #define MPU9150_DELAYCTRL_DELAY_ES_SHADOW_BIT 7 334 | #define MPU9150_DELAYCTRL_I2C_SLV4_DLY_EN_BIT 4 335 | #define MPU9150_DELAYCTRL_I2C_SLV3_DLY_EN_BIT 3 336 | #define MPU9150_DELAYCTRL_I2C_SLV2_DLY_EN_BIT 2 337 | #define MPU9150_DELAYCTRL_I2C_SLV1_DLY_EN_BIT 1 338 | #define MPU9150_DELAYCTRL_I2C_SLV0_DLY_EN_BIT 0 339 | 340 | #define MPU9150_PATHRESET_GYRO_RESET_BIT 2 341 | #define MPU9150_PATHRESET_ACCEL_RESET_BIT 1 342 | #define MPU9150_PATHRESET_TEMP_RESET_BIT 0 343 | 344 | #define MPU9150_DETECT_ACCEL_ON_DELAY_BIT 5 345 | #define MPU9150_DETECT_ACCEL_ON_DELAY_LENGTH 2 346 | #define MPU9150_DETECT_FF_COUNT_BIT 3 347 | #define MPU9150_DETECT_FF_COUNT_LENGTH 2 348 | #define MPU9150_DETECT_MOT_COUNT_BIT 1 349 | #define MPU9150_DETECT_MOT_COUNT_LENGTH 2 350 | 351 | #define MPU9150_DETECT_DECREMENT_RESET 0x0 352 | #define MPU9150_DETECT_DECREMENT_1 0x1 353 | #define MPU9150_DETECT_DECREMENT_2 0x2 354 | #define MPU9150_DETECT_DECREMENT_4 0x3 355 | 356 | #define MPU9150_USERCTRL_DMP_EN_BIT 7 357 | #define MPU9150_USERCTRL_FIFO_EN_BIT 6 358 | #define MPU9150_USERCTRL_I2C_MST_EN_BIT 5 359 | #define MPU9150_USERCTRL_I2C_IF_DIS_BIT 4 360 | #define MPU9150_USERCTRL_DMP_RESET_BIT 3 361 | #define MPU9150_USERCTRL_FIFO_RESET_BIT 2 362 | #define MPU9150_USERCTRL_I2C_MST_RESET_BIT 1 363 | #define MPU9150_USERCTRL_SIG_COND_RESET_BIT 0 364 | 365 | #define MPU9150_PWR1_DEVICE_RESET_BIT 7 366 | #define MPU9150_PWR1_SLEEP_BIT 6 367 | #define MPU9150_PWR1_CYCLE_BIT 5 368 | #define MPU9150_PWR1_TEMP_DIS_BIT 3 369 | #define MPU9150_PWR1_CLKSEL_BIT 2 370 | #define MPU9150_PWR1_CLKSEL_LENGTH 3 371 | 372 | #define MPU9150_CLOCK_INTERNAL 0x00 373 | #define MPU9150_CLOCK_PLL_XGYRO 0x01 374 | #define MPU9150_CLOCK_PLL_YGYRO 0x02 375 | #define MPU9150_CLOCK_PLL_ZGYRO 0x03 376 | #define MPU9150_CLOCK_PLL_EXT32K 0x04 377 | #define MPU9150_CLOCK_PLL_EXT19M 0x05 378 | #define MPU9150_CLOCK_KEEP_RESET 0x07 379 | 380 | #define MPU9150_PWR2_LP_WAKE_CTRL_BIT 7 381 | #define MPU9150_PWR2_LP_WAKE_CTRL_LENGTH 2 382 | #define MPU9150_PWR2_STBY_XA_BIT 5 383 | #define MPU9150_PWR2_STBY_YA_BIT 4 384 | #define MPU9150_PWR2_STBY_ZA_BIT 3 385 | #define MPU9150_PWR2_STBY_XG_BIT 2 386 | #define MPU9150_PWR2_STBY_YG_BIT 1 387 | #define MPU9150_PWR2_STBY_ZG_BIT 0 388 | 389 | #define MPU9150_WAKE_FREQ_1P25 0x0 390 | #define MPU9150_WAKE_FREQ_2P5 0x1 391 | #define MPU9150_WAKE_FREQ_5 0x2 392 | #define MPU9150_WAKE_FREQ_10 0x3 393 | 394 | #define MPU9150_BANKSEL_PRFTCH_EN_BIT 6 395 | #define MPU9150_BANKSEL_CFG_USER_BANK_BIT 5 396 | #define MPU9150_BANKSEL_MEM_SEL_BIT 4 397 | #define MPU9150_BANKSEL_MEM_SEL_LENGTH 5 398 | 399 | #define MPU9150_WHO_AM_I_BIT 6 400 | #define MPU9150_WHO_AM_I_LENGTH 6 401 | 402 | #define MPU9150_DMP_MEMORY_BANKS 8 403 | #define MPU9150_DMP_MEMORY_BANK_SIZE 256 404 | #define MPU9150_DMP_MEMORY_CHUNK_SIZE 16 405 | 406 | // note: DMP code memory blocks defined at end of header file 407 | 408 | class MPU9150 { 409 | public: 410 | MPU9150(); 411 | MPU9150(uint8_t address); 412 | 413 | void initialize(); 414 | bool testConnection(); 415 | 416 | // AUX_VDDIO register 417 | uint8_t getAuxVDDIOLevel(); 418 | void setAuxVDDIOLevel(uint8_t level); 419 | 420 | // SMPLRT_DIV register 421 | uint8_t getRate(); 422 | void setRate(uint8_t rate); 423 | 424 | // CONFIG register 425 | uint8_t getExternalFrameSync(); 426 | void setExternalFrameSync(uint8_t sync); 427 | uint8_t getDLPFMode(); 428 | void setDLPFMode(uint8_t bandwidth); 429 | 430 | // GYRO_CONFIG register 431 | uint8_t getFullScaleGyroRange(); 432 | void setFullScaleGyroRange(uint8_t range); 433 | 434 | // ACCEL_CONFIG register 435 | bool getAccelXSelfTest(); 436 | void setAccelXSelfTest(bool enabled); 437 | bool getAccelYSelfTest(); 438 | void setAccelYSelfTest(bool enabled); 439 | bool getAccelZSelfTest(); 440 | void setAccelZSelfTest(bool enabled); 441 | uint8_t getFullScaleAccelRange(); 442 | void setFullScaleAccelRange(uint8_t range); 443 | uint8_t getDHPFMode(); 444 | void setDHPFMode(uint8_t mode); 445 | 446 | // FF_THR register 447 | uint8_t getFreefallDetectionThreshold(); 448 | void setFreefallDetectionThreshold(uint8_t threshold); 449 | 450 | // FF_DUR register 451 | uint8_t getFreefallDetectionDuration(); 452 | void setFreefallDetectionDuration(uint8_t duration); 453 | 454 | // MOT_THR register 455 | uint8_t getMotionDetectionThreshold(); 456 | void setMotionDetectionThreshold(uint8_t threshold); 457 | 458 | // MOT_DUR register 459 | uint8_t getMotionDetectionDuration(); 460 | void setMotionDetectionDuration(uint8_t duration); 461 | 462 | // ZRMOT_THR register 463 | uint8_t getZeroMotionDetectionThreshold(); 464 | void setZeroMotionDetectionThreshold(uint8_t threshold); 465 | 466 | // ZRMOT_DUR register 467 | uint8_t getZeroMotionDetectionDuration(); 468 | void setZeroMotionDetectionDuration(uint8_t duration); 469 | 470 | // FIFO_EN register 471 | bool getTempFIFOEnabled(); 472 | void setTempFIFOEnabled(bool enabled); 473 | bool getXGyroFIFOEnabled(); 474 | void setXGyroFIFOEnabled(bool enabled); 475 | bool getYGyroFIFOEnabled(); 476 | void setYGyroFIFOEnabled(bool enabled); 477 | bool getZGyroFIFOEnabled(); 478 | void setZGyroFIFOEnabled(bool enabled); 479 | bool getAccelFIFOEnabled(); 480 | void setAccelFIFOEnabled(bool enabled); 481 | bool getSlave2FIFOEnabled(); 482 | void setSlave2FIFOEnabled(bool enabled); 483 | bool getSlave1FIFOEnabled(); 484 | void setSlave1FIFOEnabled(bool enabled); 485 | bool getSlave0FIFOEnabled(); 486 | void setSlave0FIFOEnabled(bool enabled); 487 | 488 | // I2C_MST_CTRL register 489 | bool getMultiMasterEnabled(); 490 | void setMultiMasterEnabled(bool enabled); 491 | bool getWaitForExternalSensorEnabled(); 492 | void setWaitForExternalSensorEnabled(bool enabled); 493 | bool getSlave3FIFOEnabled(); 494 | void setSlave3FIFOEnabled(bool enabled); 495 | bool getSlaveReadWriteTransitionEnabled(); 496 | void setSlaveReadWriteTransitionEnabled(bool enabled); 497 | uint8_t getMasterClockSpeed(); 498 | void setMasterClockSpeed(uint8_t speed); 499 | 500 | // I2C_SLV* registers (Slave 0-3) 501 | uint8_t getSlaveAddress(uint8_t num); 502 | void setSlaveAddress(uint8_t num, uint8_t address); 503 | uint8_t getSlaveRegister(uint8_t num); 504 | void setSlaveRegister(uint8_t num, uint8_t reg); 505 | bool getSlaveEnabled(uint8_t num); 506 | void setSlaveEnabled(uint8_t num, bool enabled); 507 | bool getSlaveWordByteSwap(uint8_t num); 508 | void setSlaveWordByteSwap(uint8_t num, bool enabled); 509 | bool getSlaveWriteMode(uint8_t num); 510 | void setSlaveWriteMode(uint8_t num, bool mode); 511 | bool getSlaveWordGroupOffset(uint8_t num); 512 | void setSlaveWordGroupOffset(uint8_t num, bool enabled); 513 | uint8_t getSlaveDataLength(uint8_t num); 514 | void setSlaveDataLength(uint8_t num, uint8_t length); 515 | 516 | // I2C_SLV* registers (Slave 4) 517 | uint8_t getSlave4Address(); 518 | void setSlave4Address(uint8_t address); 519 | uint8_t getSlave4Register(); 520 | void setSlave4Register(uint8_t reg); 521 | void setSlave4OutputByte(uint8_t data); 522 | bool getSlave4Enabled(); 523 | void setSlave4Enabled(bool enabled); 524 | bool getSlave4InterruptEnabled(); 525 | void setSlave4InterruptEnabled(bool enabled); 526 | bool getSlave4WriteMode(); 527 | void setSlave4WriteMode(bool mode); 528 | uint8_t getSlave4MasterDelay(); 529 | void setSlave4MasterDelay(uint8_t delay); 530 | uint8_t getSlate4InputByte(); 531 | 532 | // I2C_MST_STATUS register 533 | bool getPassthroughStatus(); 534 | bool getSlave4IsDone(); 535 | bool getLostArbitration(); 536 | bool getSlave4Nack(); 537 | bool getSlave3Nack(); 538 | bool getSlave2Nack(); 539 | bool getSlave1Nack(); 540 | bool getSlave0Nack(); 541 | 542 | // INT_PIN_CFG register 543 | bool getInterruptMode(); 544 | void setInterruptMode(bool mode); 545 | bool getInterruptDrive(); 546 | void setInterruptDrive(bool drive); 547 | bool getInterruptLatch(); 548 | void setInterruptLatch(bool latch); 549 | bool getInterruptLatchClear(); 550 | void setInterruptLatchClear(bool clear); 551 | bool getFSyncInterruptLevel(); 552 | void setFSyncInterruptLevel(bool level); 553 | bool getFSyncInterruptEnabled(); 554 | void setFSyncInterruptEnabled(bool enabled); 555 | bool getI2CBypassEnabled(); 556 | void setI2CBypassEnabled(bool enabled); 557 | bool getClockOutputEnabled(); 558 | void setClockOutputEnabled(bool enabled); 559 | 560 | // INT_ENABLE register 561 | uint8_t getIntEnabled(); 562 | void setIntEnabled(uint8_t enabled); 563 | bool getIntFreefallEnabled(); 564 | void setIntFreefallEnabled(bool enabled); 565 | bool getIntMotionEnabled(); 566 | void setIntMotionEnabled(bool enabled); 567 | bool getIntZeroMotionEnabled(); 568 | void setIntZeroMotionEnabled(bool enabled); 569 | bool getIntFIFOBufferOverflowEnabled(); 570 | void setIntFIFOBufferOverflowEnabled(bool enabled); 571 | bool getIntI2CMasterEnabled(); 572 | void setIntI2CMasterEnabled(bool enabled); 573 | bool getIntDataReadyEnabled(); 574 | void setIntDataReadyEnabled(bool enabled); 575 | 576 | // INT_STATUS register 577 | uint8_t getIntStatus(); 578 | bool getIntFreefallStatus(); 579 | bool getIntMotionStatus(); 580 | bool getIntZeroMotionStatus(); 581 | bool getIntFIFOBufferOverflowStatus(); 582 | bool getIntI2CMasterStatus(); 583 | bool getIntDataReadyStatus(); 584 | 585 | // ACCEL_*OUT_* registers 586 | 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); 587 | void getMotion6(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz); 588 | void getAcceleration(int16_t* x, int16_t* y, int16_t* z); 589 | int16_t getAccelerationX(); 590 | int16_t getAccelerationY(); 591 | int16_t getAccelerationZ(); 592 | 593 | // TEMP_OUT_* registers 594 | int16_t getTemperature(); 595 | 596 | // GYRO_*OUT_* registers 597 | void getRotation(int16_t* x, int16_t* y, int16_t* z); 598 | int16_t getRotationX(); 599 | int16_t getRotationY(); 600 | int16_t getRotationZ(); 601 | 602 | // EXT_SENS_DATA_* registers 603 | uint8_t getExternalSensorByte(int position); 604 | uint16_t getExternalSensorWord(int position); 605 | uint32_t getExternalSensorDWord(int position); 606 | 607 | // MOT_DETECT_STATUS register 608 | bool getXNegMotionDetected(); 609 | bool getXPosMotionDetected(); 610 | bool getYNegMotionDetected(); 611 | bool getYPosMotionDetected(); 612 | bool getZNegMotionDetected(); 613 | bool getZPosMotionDetected(); 614 | bool getZeroMotionDetected(); 615 | 616 | // I2C_SLV*_DO register 617 | void setSlaveOutputByte(uint8_t num, uint8_t data); 618 | 619 | // I2C_MST_DELAY_CTRL register 620 | bool getExternalShadowDelayEnabled(); 621 | void setExternalShadowDelayEnabled(bool enabled); 622 | bool getSlaveDelayEnabled(uint8_t num); 623 | void setSlaveDelayEnabled(uint8_t num, bool enabled); 624 | 625 | // SIGNAL_PATH_RESET register 626 | void resetGyroscopePath(); 627 | void resetAccelerometerPath(); 628 | void resetTemperaturePath(); 629 | 630 | // MOT_DETECT_CTRL register 631 | uint8_t getAccelerometerPowerOnDelay(); 632 | void setAccelerometerPowerOnDelay(uint8_t delay); 633 | uint8_t getFreefallDetectionCounterDecrement(); 634 | void setFreefallDetectionCounterDecrement(uint8_t decrement); 635 | uint8_t getMotionDetectionCounterDecrement(); 636 | void setMotionDetectionCounterDecrement(uint8_t decrement); 637 | 638 | // USER_CTRL register 639 | bool getFIFOEnabled(); 640 | void setFIFOEnabled(bool enabled); 641 | bool getI2CMasterModeEnabled(); 642 | void setI2CMasterModeEnabled(bool enabled); 643 | void switchSPIEnabled(bool enabled); 644 | void resetFIFO(); 645 | void resetI2CMaster(); 646 | void resetSensors(); 647 | 648 | // PWR_MGMT_1 register 649 | void reset(); 650 | bool getSleepEnabled(); 651 | void setSleepEnabled(bool enabled); 652 | bool getWakeCycleEnabled(); 653 | void setWakeCycleEnabled(bool enabled); 654 | bool getTempSensorEnabled(); 655 | void setTempSensorEnabled(bool enabled); 656 | uint8_t getClockSource(); 657 | void setClockSource(uint8_t source); 658 | 659 | // PWR_MGMT_2 register 660 | uint8_t getWakeFrequency(); 661 | void setWakeFrequency(uint8_t frequency); 662 | bool getStandbyXAccelEnabled(); 663 | void setStandbyXAccelEnabled(bool enabled); 664 | bool getStandbyYAccelEnabled(); 665 | void setStandbyYAccelEnabled(bool enabled); 666 | bool getStandbyZAccelEnabled(); 667 | void setStandbyZAccelEnabled(bool enabled); 668 | bool getStandbyXGyroEnabled(); 669 | void setStandbyXGyroEnabled(bool enabled); 670 | bool getStandbyYGyroEnabled(); 671 | void setStandbyYGyroEnabled(bool enabled); 672 | bool getStandbyZGyroEnabled(); 673 | void setStandbyZGyroEnabled(bool enabled); 674 | 675 | // FIFO_COUNT_* registers 676 | uint16_t getFIFOCount(); 677 | 678 | // FIFO_R_W register 679 | uint8_t getFIFOByte(); 680 | void setFIFOByte(uint8_t data); 681 | void getFIFOBytes(uint8_t *data, uint8_t length); 682 | 683 | // WHO_AM_I register 684 | uint8_t getDeviceID(); 685 | void setDeviceID(uint8_t id); 686 | 687 | // ======== UNDOCUMENTED/DMP REGISTERS/METHODS ======== 688 | 689 | // XG_OFFS_TC register 690 | uint8_t getOTPBankValid(); 691 | void setOTPBankValid(bool enabled); 692 | int8_t getXGyroOffset(); 693 | void setXGyroOffset(int8_t offset); 694 | 695 | // YG_OFFS_TC register 696 | int8_t getYGyroOffset(); 697 | void setYGyroOffset(int8_t offset); 698 | 699 | // ZG_OFFS_TC register 700 | int8_t getZGyroOffset(); 701 | void setZGyroOffset(int8_t offset); 702 | 703 | // X_FINE_GAIN register 704 | int8_t getXFineGain(); 705 | void setXFineGain(int8_t gain); 706 | 707 | // Y_FINE_GAIN register 708 | int8_t getYFineGain(); 709 | void setYFineGain(int8_t gain); 710 | 711 | // Z_FINE_GAIN register 712 | int8_t getZFineGain(); 713 | void setZFineGain(int8_t gain); 714 | 715 | // XA_OFFS_* registers 716 | int16_t getXAccelOffset(); 717 | void setXAccelOffset(int16_t offset); 718 | 719 | // YA_OFFS_* register 720 | int16_t getYAccelOffset(); 721 | void setYAccelOffset(int16_t offset); 722 | 723 | // ZA_OFFS_* register 724 | int16_t getZAccelOffset(); 725 | void setZAccelOffset(int16_t offset); 726 | 727 | // XG_OFFS_USR* registers 728 | int16_t getXGyroOffsetUser(); 729 | void setXGyroOffsetUser(int16_t offset); 730 | 731 | // YG_OFFS_USR* register 732 | int16_t getYGyroOffsetUser(); 733 | void setYGyroOffsetUser(int16_t offset); 734 | 735 | // ZG_OFFS_USR* register 736 | int16_t getZGyroOffsetUser(); 737 | void setZGyroOffsetUser(int16_t offset); 738 | 739 | // INT_ENABLE register (DMP functions) 740 | bool getIntPLLReadyEnabled(); 741 | void setIntPLLReadyEnabled(bool enabled); 742 | bool getIntDMPEnabled(); 743 | void setIntDMPEnabled(bool enabled); 744 | 745 | // DMP_INT_STATUS 746 | bool getDMPInt5Status(); 747 | bool getDMPInt4Status(); 748 | bool getDMPInt3Status(); 749 | bool getDMPInt2Status(); 750 | bool getDMPInt1Status(); 751 | bool getDMPInt0Status(); 752 | 753 | // INT_STATUS register (DMP functions) 754 | bool getIntPLLReadyStatus(); 755 | bool getIntDMPStatus(); 756 | 757 | // USER_CTRL register (DMP functions) 758 | bool getDMPEnabled(); 759 | void setDMPEnabled(bool enabled); 760 | void resetDMP(); 761 | 762 | // BANK_SEL register 763 | void setMemoryBank(uint8_t bank, bool prefetchEnabled=false, bool userBank=false); 764 | 765 | // MEM_START_ADDR register 766 | void setMemoryStartAddress(uint8_t address); 767 | 768 | // MEM_R_W register 769 | uint8_t readMemoryByte(); 770 | void writeMemoryByte(uint8_t data); 771 | void readMemoryBlock(uint8_t *data, uint16_t dataSize, uint8_t bank=0, uint8_t address=0); 772 | bool writeMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank=0, uint8_t address=0, bool verify=true, bool useProgMem=false); 773 | bool writeProgMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank=0, uint8_t address=0, bool verify=true); 774 | 775 | bool writeDMPConfigurationSet(const uint8_t *data, uint16_t dataSize, bool useProgMem=false); 776 | bool writeProgDMPConfigurationSet(const uint8_t *data, uint16_t dataSize); 777 | 778 | // DMP_CFG_1 register 779 | uint8_t getDMPConfig1(); 780 | void setDMPConfig1(uint8_t config); 781 | 782 | // DMP_CFG_2 register 783 | uint8_t getDMPConfig2(); 784 | void setDMPConfig2(uint8_t config); 785 | 786 | // special methods for MotionApps 2.0 implementation 787 | /*#ifdef MPU9150_INCLUDE_DMP6050 788 | uint8_t *dmpPacketBuffer; 789 | uint16_t dmpPacketSize; 790 | 791 | uint8_t dmpInitialize(); 792 | bool dmpPacketAvailable(); 793 | 794 | uint8_t dmpSetFIFORate(uint8_t fifoRate); 795 | uint8_t dmpGetFIFORate(); 796 | uint8_t dmpGetSampleStepSizeMS(); 797 | uint8_t dmpGetSampleFrequency(); 798 | int32_t dmpDecodeTemperature(int8_t tempReg); 799 | 800 | // Register callbacks after a packet of FIFO data is processed 801 | //uint8_t dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority); 802 | //uint8_t dmpUnregisterFIFORateProcess(inv_obj_func func); 803 | uint8_t dmpRunFIFORateProcesses(); 804 | 805 | // Setup FIFO for various output 806 | uint8_t dmpSendQuaternion(uint_fast16_t accuracy); 807 | uint8_t dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy); 808 | uint8_t dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy); 809 | uint8_t dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy); 810 | uint8_t dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy); 811 | uint8_t dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy); 812 | uint8_t dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy); 813 | uint8_t dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy); 814 | uint8_t dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy); 815 | uint8_t dmpSendPacketNumber(uint_fast16_t accuracy); 816 | uint8_t dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy); 817 | uint8_t dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy); 818 | 819 | // Get Fixed Point data from FIFO 820 | uint8_t dmpGetAccel(int32_t *data, const uint8_t* packet=0); 821 | uint8_t dmpGetAccel(int16_t *data, const uint8_t* packet=0); 822 | uint8_t dmpGetAccel(VectorInt16 *v, const uint8_t* packet=0); 823 | uint8_t dmpGetQuaternion(int32_t *data, const uint8_t* packet=0); 824 | uint8_t dmpGetQuaternion(int16_t *data, const uint8_t* packet=0); 825 | uint8_t dmpGetQuaternion(Quaternion *q, const uint8_t* packet=0); 826 | uint8_t dmpGet6AxisQuaternion(int32_t *data, const uint8_t* packet=0); 827 | uint8_t dmpGet6AxisQuaternion(int16_t *data, const uint8_t* packet=0); 828 | uint8_t dmpGet6AxisQuaternion(Quaternion *q, const uint8_t* packet=0); 829 | uint8_t dmpGetRelativeQuaternion(int32_t *data, const uint8_t* packet=0); 830 | uint8_t dmpGetRelativeQuaternion(int16_t *data, const uint8_t* packet=0); 831 | uint8_t dmpGetRelativeQuaternion(Quaternion *data, const uint8_t* packet=0); 832 | uint8_t dmpGetGyro(int32_t *data, const uint8_t* packet=0); 833 | uint8_t dmpGetGyro(int16_t *data, const uint8_t* packet=0); 834 | uint8_t dmpGetGyro(VectorInt16 *v, const uint8_t* packet=0); 835 | uint8_t dmpSetLinearAccelFilterCoefficient(float coef); 836 | uint8_t dmpGetLinearAccel(int32_t *data, const uint8_t* packet=0); 837 | uint8_t dmpGetLinearAccel(int16_t *data, const uint8_t* packet=0); 838 | uint8_t dmpGetLinearAccel(VectorInt16 *v, const uint8_t* packet=0); 839 | uint8_t dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity); 840 | uint8_t dmpGetLinearAccelInWorld(int32_t *data, const uint8_t* packet=0); 841 | uint8_t dmpGetLinearAccelInWorld(int16_t *data, const uint8_t* packet=0); 842 | uint8_t dmpGetLinearAccelInWorld(VectorInt16 *v, const uint8_t* packet=0); 843 | uint8_t dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q); 844 | uint8_t dmpGetGyroAndAccelSensor(int32_t *data, const uint8_t* packet=0); 845 | uint8_t dmpGetGyroAndAccelSensor(int16_t *data, const uint8_t* packet=0); 846 | uint8_t dmpGetGyroAndAccelSensor(VectorInt16 *g, VectorInt16 *a, const uint8_t* packet=0); 847 | uint8_t dmpGetGyroSensor(int32_t *data, const uint8_t* packet=0); 848 | uint8_t dmpGetGyroSensor(int16_t *data, const uint8_t* packet=0); 849 | uint8_t dmpGetGyroSensor(VectorInt16 *v, const uint8_t* packet=0); 850 | uint8_t dmpGetControlData(int32_t *data, const uint8_t* packet=0); 851 | uint8_t dmpGetTemperature(int32_t *data, const uint8_t* packet=0); 852 | uint8_t dmpGetGravity(int32_t *data, const uint8_t* packet=0); 853 | uint8_t dmpGetGravity(int16_t *data, const uint8_t* packet=0); 854 | uint8_t dmpGetGravity(VectorInt16 *v, const uint8_t* packet=0); 855 | uint8_t dmpGetGravity(VectorFloat *v, Quaternion *q); 856 | uint8_t dmpGetUnquantizedAccel(int32_t *data, const uint8_t* packet=0); 857 | uint8_t dmpGetUnquantizedAccel(int16_t *data, const uint8_t* packet=0); 858 | uint8_t dmpGetUnquantizedAccel(VectorInt16 *v, const uint8_t* packet=0); 859 | uint8_t dmpGetQuantizedAccel(int32_t *data, const uint8_t* packet=0); 860 | uint8_t dmpGetQuantizedAccel(int16_t *data, const uint8_t* packet=0); 861 | uint8_t dmpGetQuantizedAccel(VectorInt16 *v, const uint8_t* packet=0); 862 | uint8_t dmpGetExternalSensorData(int32_t *data, uint16_t size, const uint8_t* packet=0); 863 | uint8_t dmpGetEIS(int32_t *data, const uint8_t* packet=0); 864 | 865 | uint8_t dmpGetEuler(float *data, Quaternion *q); 866 | uint8_t dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity); 867 | 868 | // Get Floating Point data from FIFO 869 | uint8_t dmpGetAccelFloat(float *data, const uint8_t* packet=0); 870 | uint8_t dmpGetQuaternionFloat(float *data, const uint8_t* packet=0); 871 | 872 | uint8_t dmpProcessFIFOPacket(const unsigned char *dmpData); 873 | uint8_t dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed=NULL); 874 | 875 | uint8_t dmpSetFIFOProcessedCallback(void (*func) (void)); 876 | 877 | uint8_t dmpInitFIFOParam(); 878 | uint8_t dmpCloseFIFO(); 879 | uint8_t dmpSetGyroDataSource(uint8_t source); 880 | uint8_t dmpDecodeQuantizedAccel(); 881 | uint32_t dmpGetGyroSumOfSquare(); 882 | uint32_t dmpGetAccelSumOfSquare(); 883 | void dmpOverrideQuaternion(long *q); 884 | uint16_t dmpGetFIFOPacketSize(); 885 | #endif*/ 886 | 887 | // special methods for MotionApps 4.1 implementation 888 | // #ifdef MPU9150_INCLUDE_DMP9150 889 | uint8_t *dmpPacketBuffer; 890 | uint16_t dmpPacketSize; 891 | 892 | uint8_t dmpInitialize(); 893 | bool dmpPacketAvailable(); 894 | 895 | uint8_t dmpSetFIFORate(uint8_t fifoRate); 896 | uint8_t dmpGetFIFORate(); 897 | uint8_t dmpGetSampleStepSizeMS(); 898 | uint8_t dmpGetSampleFrequency(); 899 | int32_t dmpDecodeTemperature(int8_t tempReg); 900 | 901 | // Register callbacks after a packet of FIFO data is processed 902 | //uint8_t dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority); 903 | //uint8_t dmpUnregisterFIFORateProcess(inv_obj_func func); 904 | uint8_t dmpRunFIFORateProcesses(); 905 | 906 | // Setup FIFO for various output 907 | uint8_t dmpSendQuaternion(uint_fast16_t accuracy); 908 | uint8_t dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy); 909 | uint8_t dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy); 910 | uint8_t dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy); 911 | uint8_t dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy); 912 | uint8_t dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy); 913 | uint8_t dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy); 914 | uint8_t dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy); 915 | uint8_t dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy); 916 | uint8_t dmpSendPacketNumber(uint_fast16_t accuracy); 917 | uint8_t dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy); 918 | uint8_t dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy); 919 | 920 | // Get Fixed Point data from FIFO 921 | uint8_t dmpGetAccel(int32_t *data, const uint8_t* packet=0); 922 | uint8_t dmpGetAccel(int16_t *data, const uint8_t* packet=0); 923 | uint8_t dmpGetAccel(VectorInt16 *v, const uint8_t* packet=0); 924 | uint8_t dmpGetQuaternion(int32_t *data, const uint8_t* packet=0); 925 | uint8_t dmpGetQuaternion(int16_t *data, const uint8_t* packet=0); 926 | uint8_t dmpGetQuaternion(Quaternion *q, const uint8_t* packet=0); 927 | uint8_t dmpGet6AxisQuaternion(int32_t *data, const uint8_t* packet=0); 928 | uint8_t dmpGet6AxisQuaternion(int16_t *data, const uint8_t* packet=0); 929 | uint8_t dmpGet6AxisQuaternion(Quaternion *q, const uint8_t* packet=0); 930 | uint8_t dmpGetRelativeQuaternion(int32_t *data, const uint8_t* packet=0); 931 | uint8_t dmpGetRelativeQuaternion(int16_t *data, const uint8_t* packet=0); 932 | uint8_t dmpGetRelativeQuaternion(Quaternion *data, const uint8_t* packet=0); 933 | uint8_t dmpGetGyro(int32_t *data, const uint8_t* packet=0); 934 | uint8_t dmpGetGyro(int16_t *data, const uint8_t* packet=0); 935 | uint8_t dmpGetGyro(VectorInt16 *v, const uint8_t* packet=0); 936 | uint8_t dmpGetMag(int16_t *data, const uint8_t* packet=0); 937 | uint8_t dmpSetLinearAccelFilterCoefficient(float coef); 938 | uint8_t dmpGetLinearAccel(int32_t *data, const uint8_t* packet=0); 939 | uint8_t dmpGetLinearAccel(int16_t *data, const uint8_t* packet=0); 940 | uint8_t dmpGetLinearAccel(VectorInt16 *v, const uint8_t* packet=0); 941 | uint8_t dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity); 942 | uint8_t dmpGetLinearAccelInWorld(int32_t *data, const uint8_t* packet=0); 943 | uint8_t dmpGetLinearAccelInWorld(int16_t *data, const uint8_t* packet=0); 944 | uint8_t dmpGetLinearAccelInWorld(VectorInt16 *v, const uint8_t* packet=0); 945 | uint8_t dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q); 946 | uint8_t dmpGetGyroAndAccelSensor(int32_t *data, const uint8_t* packet=0); 947 | uint8_t dmpGetGyroAndAccelSensor(int16_t *data, const uint8_t* packet=0); 948 | uint8_t dmpGetGyroAndAccelSensor(VectorInt16 *g, VectorInt16 *a, const uint8_t* packet=0); 949 | uint8_t dmpGetGyroSensor(int32_t *data, const uint8_t* packet=0); 950 | uint8_t dmpGetGyroSensor(int16_t *data, const uint8_t* packet=0); 951 | uint8_t dmpGetGyroSensor(VectorInt16 *v, const uint8_t* packet=0); 952 | uint8_t dmpGetControlData(int32_t *data, const uint8_t* packet=0); 953 | uint8_t dmpGetTemperature(int32_t *data, const uint8_t* packet=0); 954 | uint8_t dmpGetGravity(int32_t *data, const uint8_t* packet=0); 955 | uint8_t dmpGetGravity(int16_t *data, const uint8_t* packet=0); 956 | uint8_t dmpGetGravity(VectorInt16 *v, const uint8_t* packet=0); 957 | uint8_t dmpGetGravity(VectorFloat *v, Quaternion *q); 958 | uint8_t dmpGetUnquantizedAccel(int32_t *data, const uint8_t* packet=0); 959 | uint8_t dmpGetUnquantizedAccel(int16_t *data, const uint8_t* packet=0); 960 | uint8_t dmpGetUnquantizedAccel(VectorInt16 *v, const uint8_t* packet=0); 961 | uint8_t dmpGetQuantizedAccel(int32_t *data, const uint8_t* packet=0); 962 | uint8_t dmpGetQuantizedAccel(int16_t *data, const uint8_t* packet=0); 963 | uint8_t dmpGetQuantizedAccel(VectorInt16 *v, const uint8_t* packet=0); 964 | uint8_t dmpGetExternalSensorData(int32_t *data, uint16_t size, const uint8_t* packet=0); 965 | uint8_t dmpGetEIS(int32_t *data, const uint8_t* packet=0); 966 | 967 | uint8_t dmpGetEuler(float *data, Quaternion *q); 968 | uint8_t dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity); 969 | 970 | // Get Floating Point data from FIFO 971 | uint8_t dmpGetAccelFloat(float *data, const uint8_t* packet=0); 972 | uint8_t dmpGetQuaternionFloat(float *data, const uint8_t* packet=0); 973 | 974 | uint8_t dmpProcessFIFOPacket(const unsigned char *dmpData); 975 | uint8_t dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed=NULL); 976 | 977 | uint8_t dmpSetFIFOProcessedCallback(void (*func) (void)); 978 | 979 | uint8_t dmpInitFIFOParam(); 980 | uint8_t dmpCloseFIFO(); 981 | uint8_t dmpSetGyroDataSource(uint8_t source); 982 | uint8_t dmpDecodeQuantizedAccel(); 983 | uint32_t dmpGetGyroSumOfSquare(); 984 | uint32_t dmpGetAccelSumOfSquare(); 985 | void dmpOverrideQuaternion(long *q); 986 | uint16_t dmpGetFIFOPacketSize(); 987 | // #endif 988 | 989 | private: 990 | uint8_t devAddr; 991 | uint8_t buffer[14]; 992 | }; 993 | 994 | #endif /* _MPU9150_H_ */ 995 | -------------------------------------------------------------------------------- /sensors/MPU/dmpMPU6050.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 | // 6/18/2012 by Jeff Rowberg 4 | // Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib 5 | // 6 | // Changelog: 7 | // ... - ongoing debug release 8 | 9 | /* ============================================ 10 | I2Cdev device library code is placed under the MIT license 11 | Copyright (c) 2012 Jeff Rowberg 12 | 13 | Permission is hereby granted, free of charge, to any person obtaining a copy 14 | of this software and associated documentation files (the "Software"), to deal 15 | in the Software without restriction, including without limitation the rights 16 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 17 | copies of the Software, and to permit persons to whom the Software is 18 | furnished to do so, subject to the following conditions: 19 | 20 | The above copyright notice and this permission notice shall be included in 21 | all copies or substantial portions of the Software. 22 | 23 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 24 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 25 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 26 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 27 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 28 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 29 | THE SOFTWARE. 30 | =============================================== 31 | */ 32 | 33 | #ifndef _DMP6050_H_ 34 | #define _DMP6050_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 MPU9150_INCLUDE_DMP6050 41 | 42 | #include "sensors/MPU/MPU6050.h" 43 | 44 | #if defined(__AVR__) 45 | #include 46 | #define fontdatatype uint8_t 47 | #elif defined(__PIC32MX__) 48 | #define PROGMEM 49 | #define fontdatatype const unsigned char 50 | #elif defined(__arm__) 51 | #define PROGMEM 52 | #define fontdatatype const unsigned char 53 | #endif 54 | 55 | /* Source is from the InvenSense MotionApps v2 demo code. Original source is 56 | * unavailable, unless you happen to be amazing as decompiling binary by 57 | * hand (in which case, please contact me, and I'm totally serious). 58 | * 59 | * Also, I'd like to offer many, many thanks to Noah Zerkin for all of the 60 | * DMP reverse-engineering he did to help make this bit of wizardry 61 | * possible. 62 | */ 63 | 64 | // NOTE! Enabling DEBUG adds about 3.3kB to the flash program size. 65 | // Debug output is now working even on ATMega328P MCUs (e.g. Arduino Uno) 66 | // after moving string constants to flash memory storage using the F() 67 | // compiler macro (Arduino IDE 1.0+ required). 68 | 69 | // #define DEBUG 70 | #ifdef DEBUG 71 | #define DEBUG_PRINT(x) Serial.print(x) 72 | #define DEBUG_PRINTF(x, y) Serial.print(x, y) 73 | #define DEBUG_PRINTLN(x) Serial.println(x) 74 | #define DEBUG_PRINTLNF(x, y) Serial.println(x, y) 75 | #define DEBUG_PRINTLN2(x, y) Serial.println(x, y) 76 | #else 77 | #define DEBUG_PRINT(x) 78 | #define DEBUG_PRINTF(x, y) 79 | #define DEBUG_PRINTLN(x) 80 | #define DEBUG_PRINTLNF(x, y) 81 | #endif 82 | 83 | #define MPU6050_DMP_CODE_SIZE 1929 // dmpMemory[] 84 | #define MPU6050_DMP_CONFIG_SIZE 192 // dmpConfig[] 85 | #define MPU6050_DMP_UPDATES_SIZE 47 // dmpUpdates[] 86 | 87 | /* ================================================================================================ * 88 | | Default MotionApps v2.0 42-byte FIFO packet structure: | 89 | | | 90 | | [QUAT W][ ][QUAT X][ ][QUAT Y][ ][QUAT Z][ ][GYRO X][ ][GYRO Y][ ] | 91 | | 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 | 92 | | | 93 | | [GYRO Z][ ][ACC X ][ ][ACC Y ][ ][ACC Z ][ ][ ] | 94 | | 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 | 95 | * ================================================================================================ */ 96 | 97 | // this block of memory gets written to the MPU on start-up, and it seems 98 | // to be volatile memory, so it has to be done each time (it only takes ~1 99 | // second though) 100 | const unsigned char dmpMemory[MPU6050_DMP_CODE_SIZE] PROGMEM = { 101 | // bank 0, 256 bytes 102 | 0xFB, 0x00, 0x00, 0x3E, 0x00, 0x0B, 0x00, 0x36, 0x00, 0x01, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00, 103 | 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0xFA, 0x80, 0x00, 0x0B, 0x12, 0x82, 0x00, 0x01, 104 | 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 105 | 0x00, 0x28, 0x00, 0x00, 0xFF, 0xFF, 0x45, 0x81, 0xFF, 0xFF, 0xFA, 0x72, 0x00, 0x00, 0x00, 0x00, 106 | 0x00, 0x00, 0x03, 0xE8, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x7F, 0xFF, 0xFF, 0xFE, 0x80, 0x01, 107 | 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 108 | 0x00, 0x3E, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xCA, 0xE3, 0x09, 0x3E, 0x80, 0x00, 0x00, 109 | 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00, 110 | 0x41, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x0B, 0x2A, 0x00, 0x00, 0x16, 0x55, 0x00, 0x00, 0x21, 0x82, 111 | 0xFD, 0x87, 0x26, 0x50, 0xFD, 0x80, 0x00, 0x00, 0x00, 0x1F, 0x00, 0x00, 0x00, 0x05, 0x80, 0x00, 112 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 113 | 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x6F, 0x00, 0x02, 0x65, 0x32, 0x00, 0x00, 0x5E, 0xC0, 114 | 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 115 | 0xFB, 0x8C, 0x6F, 0x5D, 0xFD, 0x5D, 0x08, 0xD9, 0x00, 0x7C, 0x73, 0x3B, 0x00, 0x6C, 0x12, 0xCC, 116 | 0x32, 0x00, 0x13, 0x9D, 0x32, 0x00, 0xD0, 0xD6, 0x32, 0x00, 0x08, 0x00, 0x40, 0x00, 0x01, 0xF4, 117 | 0xFF, 0xE6, 0x80, 0x79, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0xD0, 0xD6, 0x00, 0x00, 0x27, 0x10, 118 | 119 | // bank 1, 256 bytes 120 | 0xFB, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 121 | 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x00, 122 | 0x00, 0x00, 0xFA, 0x36, 0xFF, 0xBC, 0x30, 0x8E, 0x00, 0x05, 0xFB, 0xF0, 0xFF, 0xD9, 0x5B, 0xC8, 123 | 0xFF, 0xD0, 0x9A, 0xBE, 0x00, 0x00, 0x10, 0xA9, 0xFF, 0xF4, 0x1E, 0xB2, 0x00, 0xCE, 0xBB, 0xF7, 124 | 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x04, 0x00, 0x02, 0x00, 0x02, 0x02, 0x00, 0x00, 0x0C, 125 | 0xFF, 0xC2, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0xCF, 0x80, 0x00, 0x40, 0x00, 0x00, 0x00, 126 | 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14, 127 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 128 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 129 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 130 | 0x00, 0x00, 0x00, 0x00, 0x03, 0x3F, 0x68, 0xB6, 0x79, 0x35, 0x28, 0xBC, 0xC6, 0x7E, 0xD1, 0x6C, 131 | 0x80, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0xB2, 0x6A, 0x00, 0x00, 0x00, 0x00, 132 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3F, 0xF0, 0x00, 0x00, 0x00, 0x30, 133 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 134 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 135 | 0x00, 0x00, 0x25, 0x4D, 0x00, 0x2F, 0x70, 0x6D, 0x00, 0x00, 0x05, 0xAE, 0x00, 0x0C, 0x02, 0xD0, 136 | 137 | // bank 2, 256 bytes 138 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 139 | 0x00, 0x00, 0x01, 0x00, 0x00, 0x44, 0x00, 0x00, 0x00, 0x00, 0x0C, 0x00, 0x00, 0x00, 0x01, 0x00, 140 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x54, 0x00, 0x00, 0xFF, 0xEF, 0x00, 0x00, 141 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 142 | 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 143 | 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 144 | 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 145 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 146 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 147 | 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 148 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 149 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 150 | 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 151 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 152 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 153 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 154 | 155 | // bank 3, 256 bytes 156 | 0xD8, 0xDC, 0xBA, 0xA2, 0xF1, 0xDE, 0xB2, 0xB8, 0xB4, 0xA8, 0x81, 0x91, 0xF7, 0x4A, 0x90, 0x7F, 157 | 0x91, 0x6A, 0xF3, 0xF9, 0xDB, 0xA8, 0xF9, 0xB0, 0xBA, 0xA0, 0x80, 0xF2, 0xCE, 0x81, 0xF3, 0xC2, 158 | 0xF1, 0xC1, 0xF2, 0xC3, 0xF3, 0xCC, 0xA2, 0xB2, 0x80, 0xF1, 0xC6, 0xD8, 0x80, 0xBA, 0xA7, 0xDF, 159 | 0xDF, 0xDF, 0xF2, 0xA7, 0xC3, 0xCB, 0xC5, 0xB6, 0xF0, 0x87, 0xA2, 0x94, 0x24, 0x48, 0x70, 0x3C, 160 | 0x95, 0x40, 0x68, 0x34, 0x58, 0x9B, 0x78, 0xA2, 0xF1, 0x83, 0x92, 0x2D, 0x55, 0x7D, 0xD8, 0xB1, 161 | 0xB4, 0xB8, 0xA1, 0xD0, 0x91, 0x80, 0xF2, 0x70, 0xF3, 0x70, 0xF2, 0x7C, 0x80, 0xA8, 0xF1, 0x01, 162 | 0xB0, 0x98, 0x87, 0xD9, 0x43, 0xD8, 0x86, 0xC9, 0x88, 0xBA, 0xA1, 0xF2, 0x0E, 0xB8, 0x97, 0x80, 163 | 0xF1, 0xA9, 0xDF, 0xDF, 0xDF, 0xAA, 0xDF, 0xDF, 0xDF, 0xF2, 0xAA, 0xC5, 0xCD, 0xC7, 0xA9, 0x0C, 164 | 0xC9, 0x2C, 0x97, 0x97, 0x97, 0x97, 0xF1, 0xA9, 0x89, 0x26, 0x46, 0x66, 0xB0, 0xB4, 0xBA, 0x80, 165 | 0xAC, 0xDE, 0xF2, 0xCA, 0xF1, 0xB2, 0x8C, 0x02, 0xA9, 0xB6, 0x98, 0x00, 0x89, 0x0E, 0x16, 0x1E, 166 | 0xB8, 0xA9, 0xB4, 0x99, 0x2C, 0x54, 0x7C, 0xB0, 0x8A, 0xA8, 0x96, 0x36, 0x56, 0x76, 0xF1, 0xB9, 167 | 0xAF, 0xB4, 0xB0, 0x83, 0xC0, 0xB8, 0xA8, 0x97, 0x11, 0xB1, 0x8F, 0x98, 0xB9, 0xAF, 0xF0, 0x24, 168 | 0x08, 0x44, 0x10, 0x64, 0x18, 0xF1, 0xA3, 0x29, 0x55, 0x7D, 0xAF, 0x83, 0xB5, 0x93, 0xAF, 0xF0, 169 | 0x00, 0x28, 0x50, 0xF1, 0xA3, 0x86, 0x9F, 0x61, 0xA6, 0xDA, 0xDE, 0xDF, 0xD9, 0xFA, 0xA3, 0x86, 170 | 0x96, 0xDB, 0x31, 0xA6, 0xD9, 0xF8, 0xDF, 0xBA, 0xA6, 0x8F, 0xC2, 0xC5, 0xC7, 0xB2, 0x8C, 0xC1, 171 | 0xB8, 0xA2, 0xDF, 0xDF, 0xDF, 0xA3, 0xDF, 0xDF, 0xDF, 0xD8, 0xD8, 0xF1, 0xB8, 0xA8, 0xB2, 0x86, 172 | 173 | // bank 4, 256 bytes 174 | 0xB4, 0x98, 0x0D, 0x35, 0x5D, 0xB8, 0xAA, 0x98, 0xB0, 0x87, 0x2D, 0x35, 0x3D, 0xB2, 0xB6, 0xBA, 175 | 0xAF, 0x8C, 0x96, 0x19, 0x8F, 0x9F, 0xA7, 0x0E, 0x16, 0x1E, 0xB4, 0x9A, 0xB8, 0xAA, 0x87, 0x2C, 176 | 0x54, 0x7C, 0xB9, 0xA3, 0xDE, 0xDF, 0xDF, 0xA3, 0xB1, 0x80, 0xF2, 0xC4, 0xCD, 0xC9, 0xF1, 0xB8, 177 | 0xA9, 0xB4, 0x99, 0x83, 0x0D, 0x35, 0x5D, 0x89, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0xB5, 0x93, 0xA3, 178 | 0x0E, 0x16, 0x1E, 0xA9, 0x2C, 0x54, 0x7C, 0xB8, 0xB4, 0xB0, 0xF1, 0x97, 0x83, 0xA8, 0x11, 0x84, 179 | 0xA5, 0x09, 0x98, 0xA3, 0x83, 0xF0, 0xDA, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xD8, 0xF1, 0xA5, 180 | 0x29, 0x55, 0x7D, 0xA5, 0x85, 0x95, 0x02, 0x1A, 0x2E, 0x3A, 0x56, 0x5A, 0x40, 0x48, 0xF9, 0xF3, 181 | 0xA3, 0xD9, 0xF8, 0xF0, 0x98, 0x83, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0x97, 0x82, 0xA8, 0xF1, 182 | 0x11, 0xF0, 0x98, 0xA2, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xDA, 0xF3, 0xDE, 0xD8, 0x83, 0xA5, 183 | 0x94, 0x01, 0xD9, 0xA3, 0x02, 0xF1, 0xA2, 0xC3, 0xC5, 0xC7, 0xD8, 0xF1, 0x84, 0x92, 0xA2, 0x4D, 184 | 0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9, 185 | 0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0x93, 0xA3, 0x4D, 186 | 0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9, 187 | 0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0xA8, 0x8A, 0x9A, 188 | 0xF0, 0x28, 0x50, 0x78, 0x9E, 0xF3, 0x88, 0x18, 0xF1, 0x9F, 0x1D, 0x98, 0xA8, 0xD9, 0x08, 0xD8, 189 | 0xC8, 0x9F, 0x12, 0x9E, 0xF3, 0x15, 0xA8, 0xDA, 0x12, 0x10, 0xD8, 0xF1, 0xAF, 0xC8, 0x97, 0x87, 190 | 191 | // bank 5, 256 bytes 192 | 0x34, 0xB5, 0xB9, 0x94, 0xA4, 0x21, 0xF3, 0xD9, 0x22, 0xD8, 0xF2, 0x2D, 0xF3, 0xD9, 0x2A, 0xD8, 193 | 0xF2, 0x35, 0xF3, 0xD9, 0x32, 0xD8, 0x81, 0xA4, 0x60, 0x60, 0x61, 0xD9, 0x61, 0xD8, 0x6C, 0x68, 194 | 0x69, 0xD9, 0x69, 0xD8, 0x74, 0x70, 0x71, 0xD9, 0x71, 0xD8, 0xB1, 0xA3, 0x84, 0x19, 0x3D, 0x5D, 195 | 0xA3, 0x83, 0x1A, 0x3E, 0x5E, 0x93, 0x10, 0x30, 0x81, 0x10, 0x11, 0xB8, 0xB0, 0xAF, 0x8F, 0x94, 196 | 0xF2, 0xDA, 0x3E, 0xD8, 0xB4, 0x9A, 0xA8, 0x87, 0x29, 0xDA, 0xF8, 0xD8, 0x87, 0x9A, 0x35, 0xDA, 197 | 0xF8, 0xD8, 0x87, 0x9A, 0x3D, 0xDA, 0xF8, 0xD8, 0xB1, 0xB9, 0xA4, 0x98, 0x85, 0x02, 0x2E, 0x56, 198 | 0xA5, 0x81, 0x00, 0x0C, 0x14, 0xA3, 0x97, 0xB0, 0x8A, 0xF1, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9, 199 | 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x84, 0x0D, 0xDA, 0x0E, 0xD8, 0xA3, 0x29, 0x83, 0xDA, 200 | 0x2C, 0x0E, 0xD8, 0xA3, 0x84, 0x49, 0x83, 0xDA, 0x2C, 0x4C, 0x0E, 0xD8, 0xB8, 0xB0, 0xA8, 0x8A, 201 | 0x9A, 0xF5, 0x20, 0xAA, 0xDA, 0xDF, 0xD8, 0xA8, 0x40, 0xAA, 0xD0, 0xDA, 0xDE, 0xD8, 0xA8, 0x60, 202 | 0xAA, 0xDA, 0xD0, 0xDF, 0xD8, 0xF1, 0x97, 0x86, 0xA8, 0x31, 0x9B, 0x06, 0x99, 0x07, 0xAB, 0x97, 203 | 0x28, 0x88, 0x9B, 0xF0, 0x0C, 0x20, 0x14, 0x40, 0xB8, 0xB0, 0xB4, 0xA8, 0x8C, 0x9C, 0xF0, 0x04, 204 | 0x28, 0x51, 0x79, 0x1D, 0x30, 0x14, 0x38, 0xB2, 0x82, 0xAB, 0xD0, 0x98, 0x2C, 0x50, 0x50, 0x78, 205 | 0x78, 0x9B, 0xF1, 0x1A, 0xB0, 0xF0, 0x8A, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x8B, 0x29, 0x51, 0x79, 206 | 0x8A, 0x24, 0x70, 0x59, 0x8B, 0x20, 0x58, 0x71, 0x8A, 0x44, 0x69, 0x38, 0x8B, 0x39, 0x40, 0x68, 207 | 0x8A, 0x64, 0x48, 0x31, 0x8B, 0x30, 0x49, 0x60, 0xA5, 0x88, 0x20, 0x09, 0x71, 0x58, 0x44, 0x68, 208 | 209 | // bank 6, 256 bytes 210 | 0x11, 0x39, 0x64, 0x49, 0x30, 0x19, 0xF1, 0xAC, 0x00, 0x2C, 0x54, 0x7C, 0xF0, 0x8C, 0xA8, 0x04, 211 | 0x28, 0x50, 0x78, 0xF1, 0x88, 0x97, 0x26, 0xA8, 0x59, 0x98, 0xAC, 0x8C, 0x02, 0x26, 0x46, 0x66, 212 | 0xF0, 0x89, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31, 213 | 0xA9, 0x88, 0x09, 0x20, 0x59, 0x70, 0xAB, 0x11, 0x38, 0x40, 0x69, 0xA8, 0x19, 0x31, 0x48, 0x60, 214 | 0x8C, 0xA8, 0x3C, 0x41, 0x5C, 0x20, 0x7C, 0x00, 0xF1, 0x87, 0x98, 0x19, 0x86, 0xA8, 0x6E, 0x76, 215 | 0x7E, 0xA9, 0x99, 0x88, 0x2D, 0x55, 0x7D, 0x9E, 0xB9, 0xA3, 0x8A, 0x22, 0x8A, 0x6E, 0x8A, 0x56, 216 | 0x8A, 0x5E, 0x9F, 0xB1, 0x83, 0x06, 0x26, 0x46, 0x66, 0x0E, 0x2E, 0x4E, 0x6E, 0x9D, 0xB8, 0xAD, 217 | 0x00, 0x2C, 0x54, 0x7C, 0xF2, 0xB1, 0x8C, 0xB4, 0x99, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0x81, 0x91, 218 | 0xAC, 0x38, 0xAD, 0x3A, 0xB5, 0x83, 0x91, 0xAC, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9, 0x48, 0xD8, 219 | 0x6D, 0xD9, 0x68, 0xD8, 0x8C, 0x9D, 0xAE, 0x29, 0xD9, 0x04, 0xAE, 0xD8, 0x51, 0xD9, 0x04, 0xAE, 220 | 0xD8, 0x79, 0xD9, 0x04, 0xD8, 0x81, 0xF3, 0x9D, 0xAD, 0x00, 0x8D, 0xAE, 0x19, 0x81, 0xAD, 0xD9, 221 | 0x01, 0xD8, 0xF2, 0xAE, 0xDA, 0x26, 0xD8, 0x8E, 0x91, 0x29, 0x83, 0xA7, 0xD9, 0xAD, 0xAD, 0xAD, 222 | 0xAD, 0xF3, 0x2A, 0xD8, 0xD8, 0xF1, 0xB0, 0xAC, 0x89, 0x91, 0x3E, 0x5E, 0x76, 0xF3, 0xAC, 0x2E, 223 | 0x2E, 0xF1, 0xB1, 0x8C, 0x5A, 0x9C, 0xAC, 0x2C, 0x28, 0x28, 0x28, 0x9C, 0xAC, 0x30, 0x18, 0xA8, 224 | 0x98, 0x81, 0x28, 0x34, 0x3C, 0x97, 0x24, 0xA7, 0x28, 0x34, 0x3C, 0x9C, 0x24, 0xF2, 0xB0, 0x89, 225 | 0xAC, 0x91, 0x2C, 0x4C, 0x6C, 0x8A, 0x9B, 0x2D, 0xD9, 0xD8, 0xD8, 0x51, 0xD9, 0xD8, 0xD8, 0x79, 226 | 227 | // bank 7, 138 bytes (remainder) 228 | 0xD9, 0xD8, 0xD8, 0xF1, 0x9E, 0x88, 0xA3, 0x31, 0xDA, 0xD8, 0xD8, 0x91, 0x2D, 0xD9, 0x28, 0xD8, 229 | 0x4D, 0xD9, 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x83, 0x93, 0x35, 0x3D, 0x80, 0x25, 0xDA, 230 | 0xD8, 0xD8, 0x85, 0x69, 0xDA, 0xD8, 0xD8, 0xB4, 0x93, 0x81, 0xA3, 0x28, 0x34, 0x3C, 0xF3, 0xAB, 231 | 0x8B, 0xF8, 0xA3, 0x91, 0xB6, 0x09, 0xB4, 0xD9, 0xAB, 0xDE, 0xFA, 0xB0, 0x87, 0x9C, 0xB9, 0xA3, 232 | 0xDD, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x95, 0xF1, 0xA3, 0xA3, 0xA3, 0x9D, 0xF1, 0xA3, 0xA3, 0xA3, 233 | 0xA3, 0xF2, 0xA3, 0xB4, 0x90, 0x80, 0xF2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 234 | 0xA3, 0xB2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xB0, 0x87, 0xB5, 0x99, 0xF1, 0xA3, 0xA3, 0xA3, 235 | 0x98, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x97, 0xA3, 0xA3, 0xA3, 0xA3, 0xF3, 0x9B, 0xA3, 0xA3, 0xDC, 236 | 0xB9, 0xA7, 0xF1, 0x26, 0x26, 0x26, 0xD8, 0xD8, 0xFF 237 | }; 238 | 239 | // thanks to Noah Zerkin for piecing this stuff together! 240 | const unsigned char dmpConfig[MPU6050_DMP_CONFIG_SIZE] PROGMEM = { 241 | // BANK OFFSET LENGTH [DATA] 242 | 0x03, 0x7B, 0x03, 0x4C, 0xCD, 0x6C, // FCFG_1 inv_set_gyro_calibration 243 | 0x03, 0xAB, 0x03, 0x36, 0x56, 0x76, // FCFG_3 inv_set_gyro_calibration 244 | 0x00, 0x68, 0x04, 0x02, 0xCB, 0x47, 0xA2, // D_0_104 inv_set_gyro_calibration 245 | 0x02, 0x18, 0x04, 0x00, 0x05, 0x8B, 0xC1, // D_0_24 inv_set_gyro_calibration 246 | 0x01, 0x0C, 0x04, 0x00, 0x00, 0x00, 0x00, // D_1_152 inv_set_accel_calibration 247 | 0x03, 0x7F, 0x06, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97, // FCFG_2 inv_set_accel_calibration 248 | 0x03, 0x89, 0x03, 0x26, 0x46, 0x66, // FCFG_7 inv_set_accel_calibration 249 | 0x00, 0x6C, 0x02, 0x20, 0x00, // D_0_108 inv_set_accel_calibration 250 | 0x02, 0x40, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_00 inv_set_compass_calibration 251 | 0x02, 0x44, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_01 252 | 0x02, 0x48, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_02 253 | 0x02, 0x4C, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_10 254 | 0x02, 0x50, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_11 255 | 0x02, 0x54, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_12 256 | 0x02, 0x58, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_20 257 | 0x02, 0x5C, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_21 258 | 0x02, 0xBC, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_22 259 | 0x01, 0xEC, 0x04, 0x00, 0x00, 0x40, 0x00, // D_1_236 inv_apply_endian_accel 260 | 0x03, 0x7F, 0x06, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97, // FCFG_2 inv_set_mpu_sensors 261 | 0x04, 0x02, 0x03, 0x0D, 0x35, 0x5D, // CFG_MOTION_BIAS inv_turn_on_bias_from_no_motion 262 | 0x04, 0x09, 0x04, 0x87, 0x2D, 0x35, 0x3D, // FCFG_5 inv_set_bias_update 263 | 0x00, 0xA3, 0x01, 0x00, // D_0_163 inv_set_dead_zone 264 | // SPECIAL 0x01 = enable interrupts 265 | 0x00, 0x00, 0x00, 0x01, // SET INT_ENABLE at i=22, SPECIAL INSTRUCTION 266 | 0x07, 0x86, 0x01, 0xFE, // CFG_6 inv_set_fifo_interupt 267 | 0x07, 0x41, 0x05, 0xF1, 0x20, 0x28, 0x30, 0x38, // CFG_8 inv_send_quaternion 268 | 0x07, 0x7E, 0x01, 0x30, // CFG_16 inv_set_footer 269 | 0x07, 0x46, 0x01, 0x9A, // CFG_GYRO_SOURCE inv_send_gyro 270 | 0x07, 0x47, 0x04, 0xF1, 0x28, 0x30, 0x38, // CFG_9 inv_send_gyro -> inv_construct3_fifo 271 | 0x07, 0x6C, 0x04, 0xF1, 0x28, 0x30, 0x38, // CFG_12 inv_send_accel -> inv_construct3_fifo 272 | 0x02, 0x16, 0x02, 0x00, 0x01 // D_0_22 inv_set_fifo_rate 273 | 274 | // This very last 0x01 WAS a 0x09, which drops the FIFO rate down to 20 Hz. 0x07 is 25 Hz, 275 | // 0x01 is 100Hz. Going faster than 100Hz (0x00=200Hz) tends to result in very noisy data. 276 | // DMP output frequency is calculated easily using this equation: (200Hz / (1 + value)) 277 | 278 | // It is important to make sure the host processor can keep up with reading and processing 279 | // the FIFO output at the desired rate. Handling FIFO overflow cleanly is also a good idea. 280 | }; 281 | 282 | const unsigned char dmpUpdates[MPU6050_DMP_UPDATES_SIZE] PROGMEM = { 283 | 0x01, 0xB2, 0x02, 0xFF, 0xFF, 284 | 0x01, 0x90, 0x04, 0x09, 0x23, 0xA1, 0x35, 285 | 0x01, 0x6A, 0x02, 0x06, 0x00, 286 | 0x01, 0x60, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 287 | 0x00, 0x60, 0x04, 0x40, 0x00, 0x00, 0x00, 288 | 0x01, 0x62, 0x02, 0x00, 0x00, 289 | 0x00, 0x60, 0x04, 0x00, 0x40, 0x00, 0x00 290 | }; 291 | 292 | uint8_t MPU6050::dmpInitialize() { 293 | // reset device 294 | DEBUG_PRINTLN(("\n\nResetting MPU6050...")); 295 | reset(); 296 | delay(30); // wait after reset 297 | 298 | // enable sleep mode and wake cycle 299 | /*Serial.println(("Enabling sleep mode...")); 300 | setSleepEnabled(true); 301 | Serial.println(("Enabling wake cycle...")); 302 | setWakeCycleEnabled(true);*/ 303 | 304 | // disable sleep mode 305 | DEBUG_PRINTLN(("Disabling sleep mode...")); 306 | setSleepEnabled(false); 307 | 308 | // get MPU hardware revision 309 | DEBUG_PRINTLN(("Selecting user bank 16...")); 310 | setMemoryBank(0x10, true, true); 311 | DEBUG_PRINTLN(("Selecting memory byte 6...")); 312 | setMemoryStartAddress(0x06); 313 | DEBUG_PRINTLN(("Checking hardware revision...")); 314 | uint8_t hwRevision = readMemoryByte(); 315 | DEBUG_PRINT(("Revision @ user[16][6] = ")); 316 | DEBUG_PRINTLN2(hwRevision, HEX); 317 | DEBUG_PRINTLN(("Resetting memory bank selection to 0...")); 318 | setMemoryBank(0, false, false); 319 | 320 | // check OTP bank valid 321 | DEBUG_PRINTLN(("Reading OTP bank valid flag...")); 322 | uint8_t otpValid = getOTPBankValid(); 323 | DEBUG_PRINT(("OTP bank is ")); 324 | DEBUG_PRINTLN(otpValid ? ("valid!") : ("invalid!")); 325 | 326 | // get X/Y/Z gyro offsets 327 | DEBUG_PRINTLN(("Reading gyro offset values...")); 328 | int8_t xgOffset = getXGyroOffset(); 329 | int8_t ygOffset = getYGyroOffset(); 330 | int8_t zgOffset = getZGyroOffset(); 331 | DEBUG_PRINT(("X gyro offset = ")); 332 | DEBUG_PRINTLN(xgOffset); 333 | DEBUG_PRINT(("Y gyro offset = ")); 334 | DEBUG_PRINTLN(ygOffset); 335 | DEBUG_PRINT(("Z gyro offset = ")); 336 | DEBUG_PRINTLN(zgOffset); 337 | 338 | // setup weird slave stuff (?) 339 | DEBUG_PRINTLN(("Setting slave 0 address to 0x7F...")); 340 | setSlaveAddress(0, 0x7F); 341 | DEBUG_PRINTLN(("Disabling I2C Master mode...")); 342 | setI2CMasterModeEnabled(false); 343 | DEBUG_PRINTLN(("Setting slave 0 address to 0x68 (self)...")); 344 | setSlaveAddress(0, 0x68); 345 | DEBUG_PRINTLN(("Resetting I2C Master control...")); 346 | resetI2CMaster(); 347 | delay(20); 348 | 349 | // load DMP code into memory banks 350 | DEBUG_PRINT(("Writing DMP code to MPU memory banks (")); 351 | DEBUG_PRINT(MPU6050_DMP_CODE_SIZE); 352 | DEBUG_PRINTLN((" bytes)")); 353 | if (writeProgMemoryBlock(dmpMemory, MPU6050_DMP_CODE_SIZE)) { 354 | DEBUG_PRINTLN(("Success! DMP code written and verified.")); 355 | 356 | // write DMP configuration 357 | DEBUG_PRINT(("Writing DMP configuration to MPU memory banks (")); 358 | DEBUG_PRINT(MPU6050_DMP_CONFIG_SIZE); 359 | DEBUG_PRINTLN((" bytes in config def)")); 360 | if (writeProgDMPConfigurationSet(dmpConfig, MPU6050_DMP_CONFIG_SIZE)) { 361 | DEBUG_PRINTLN(("Success! DMP configuration written and verified.")); 362 | 363 | DEBUG_PRINTLN(("Setting clock source to Z Gyro...")); 364 | setClockSource(MPU6050_CLOCK_PLL_ZGYRO); 365 | 366 | DEBUG_PRINTLN(("Setting DMP and FIFO_OFLOW interrupts enabled...")); 367 | setIntEnabled(0x12); 368 | 369 | DEBUG_PRINTLN(("Setting sample rate to 200Hz...")); 370 | setRate(4); // 1khz / (1 + 4) = 200 Hz 371 | 372 | DEBUG_PRINTLN(("Setting external frame sync to TEMP_OUT_L[0]...")); 373 | setExternalFrameSync(MPU6050_EXT_SYNC_TEMP_OUT_L); 374 | 375 | DEBUG_PRINTLN(("Setting DLPF bandwidth to 42Hz...")); 376 | setDLPFMode(MPU6050_DLPF_BW_42); 377 | 378 | DEBUG_PRINTLN(("Setting gyro sensitivity to +/- 2000 deg/sec...")); 379 | setFullScaleGyroRange(MPU6050_GYRO_FS_2000); 380 | 381 | DEBUG_PRINTLN(("Setting DMP configuration bytes (function unknown)...")); 382 | setDMPConfig1(0x03); 383 | setDMPConfig2(0x00); 384 | 385 | DEBUG_PRINTLN(("Clearing OTP Bank flag...")); 386 | setOTPBankValid(false); 387 | 388 | DEBUG_PRINTLN(("Setting X/Y/Z gyro offsets to previous values...")); 389 | setXGyroOffset(xgOffset); 390 | setYGyroOffset(ygOffset); 391 | setZGyroOffset(zgOffset); 392 | 393 | DEBUG_PRINTLN(("Setting X/Y/Z gyro user offsets to zero...")); 394 | setXGyroOffsetUser(0); 395 | setYGyroOffsetUser(0); 396 | setZGyroOffsetUser(0); 397 | 398 | DEBUG_PRINTLN(("Writing final memory update 1/7 (function unknown)...")); 399 | uint8_t dmpUpdate[16], j; 400 | uint16_t pos = 0; 401 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = dmpUpdates[pos]; // EDITED 402 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 403 | 404 | DEBUG_PRINTLN(("Writing final memory update 2/7 (function unknown)...")); 405 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = dmpUpdates[pos]; // EDITED 406 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 407 | 408 | DEBUG_PRINTLN(("Resetting FIFO...")); 409 | resetFIFO(); 410 | 411 | DEBUG_PRINTLN(("Reading FIFO count...")); 412 | uint8_t fifoCount = getFIFOCount(); 413 | uint8_t fifoBuffer[128]; 414 | 415 | DEBUG_PRINT(("Current FIFO count=")); 416 | DEBUG_PRINTLN(fifoCount); 417 | getFIFOBytes(fifoBuffer, fifoCount); 418 | 419 | DEBUG_PRINTLN(("Setting motion detection threshold to 2...")); 420 | setMotionDetectionThreshold(2); 421 | 422 | DEBUG_PRINTLN(("Setting zero-motion detection threshold to 156...")); 423 | setZeroMotionDetectionThreshold(156); 424 | 425 | DEBUG_PRINTLN(("Setting motion detection duration to 80...")); 426 | setMotionDetectionDuration(80); 427 | 428 | DEBUG_PRINTLN(("Setting zero-motion detection duration to 0...")); 429 | setZeroMotionDetectionDuration(0); 430 | 431 | DEBUG_PRINTLN(("Resetting FIFO...")); 432 | resetFIFO(); 433 | 434 | DEBUG_PRINTLN(("Enabling FIFO...")); 435 | setFIFOEnabled(true); 436 | 437 | DEBUG_PRINTLN(("Enabling DMP...")); 438 | setDMPEnabled(true); 439 | 440 | DEBUG_PRINTLN(("Resetting DMP...")); 441 | resetDMP(); 442 | 443 | DEBUG_PRINTLN(("Writing final memory update 3/7 (function unknown)...")); 444 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = dmpUpdates[pos]; 445 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 446 | 447 | DEBUG_PRINTLN(("Writing final memory update 4/7 (function unknown)...")); 448 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = dmpUpdates[pos]; 449 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 450 | 451 | DEBUG_PRINTLN(("Writing final memory update 5/7 (function unknown)...")); 452 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = dmpUpdates[pos]; 453 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 454 | 455 | DEBUG_PRINTLN(("Waiting for FIFO count > 2...")); 456 | while ((fifoCount = getFIFOCount()) < 3); 457 | 458 | DEBUG_PRINT(("Current FIFO count=")); 459 | DEBUG_PRINTLN(fifoCount); 460 | DEBUG_PRINTLN(("Reading FIFO data...")); 461 | getFIFOBytes(fifoBuffer, fifoCount); 462 | 463 | DEBUG_PRINTLN(("Reading interrupt status...")); 464 | uint8_t mpuIntStatus = getIntStatus(); 465 | 466 | DEBUG_PRINT(("Current interrupt status=")); 467 | DEBUG_PRINTLN2(mpuIntStatus, HEX); 468 | 469 | DEBUG_PRINTLN(("Reading final memory update 6/7 (function unknown)...")); 470 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = dmpUpdates[pos]; 471 | readMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 472 | 473 | DEBUG_PRINTLN(("Waiting for FIFO count > 2...")); 474 | while ((fifoCount = getFIFOCount()) < 3); 475 | 476 | DEBUG_PRINT(("Current FIFO count=")); 477 | DEBUG_PRINTLN(fifoCount); 478 | 479 | DEBUG_PRINTLN(("Reading FIFO data...")); 480 | getFIFOBytes(fifoBuffer, fifoCount); 481 | 482 | DEBUG_PRINTLN(("Reading interrupt status...")); 483 | mpuIntStatus = getIntStatus(); 484 | 485 | DEBUG_PRINT(("Current interrupt status=")); 486 | DEBUG_PRINTLN2(mpuIntStatus, HEX); 487 | 488 | DEBUG_PRINTLN(("Writing final memory update 7/7 (function unknown)...")); 489 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = dmpUpdates[pos]; 490 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 491 | 492 | DEBUG_PRINTLN(("DMP is good to go! Finally.")); 493 | 494 | DEBUG_PRINTLN(("Disabling DMP (you turn it on later)...")); 495 | setDMPEnabled(false); 496 | 497 | DEBUG_PRINTLN("Setting up internal 42-byte (default) DMP packet buffer..."); 498 | dmpPacketSize = 42; 499 | /*if ((dmpPacketBuffer = (uint8_t *)malloc(42)) == 0) { 500 | return 3; // TODO: proper error code for no memory 501 | }*/ 502 | 503 | DEBUG_PRINTLN(("Resetting FIFO and clearing INT status one last time...")); 504 | resetFIFO(); 505 | getIntStatus(); 506 | } else { 507 | DEBUG_PRINTLN(("ERROR! DMP configuration verification failed.")); 508 | return 2; // configuration block loading failed 509 | } 510 | } else { 511 | DEBUG_PRINTLN(("ERROR! DMP code verification failed.")); 512 | return 1; // main binary block loading failed 513 | } 514 | return 0; // success 515 | } 516 | 517 | bool MPU6050::dmpPacketAvailable() { 518 | return getFIFOCount() >= dmpGetFIFOPacketSize(); 519 | } 520 | 521 | // uint8_t MPU6050::dmpSetFIFORate(uint8_t fifoRate); 522 | // uint8_t MPU6050::dmpGetFIFORate(); 523 | // uint8_t MPU6050::dmpGetSampleStepSizeMS(); 524 | // uint8_t MPU6050::dmpGetSampleFrequency(); 525 | // int32_t MPU6050::dmpDecodeTemperature(int8_t tempReg); 526 | 527 | //uint8_t MPU6050::dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority); 528 | //uint8_t MPU6050::dmpUnregisterFIFORateProcess(inv_obj_func func); 529 | //uint8_t MPU6050::dmpRunFIFORateProcesses(); 530 | 531 | // uint8_t MPU6050::dmpSendQuaternion(uint_fast16_t accuracy); 532 | // uint8_t MPU6050::dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy); 533 | // uint8_t MPU6050::dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy); 534 | // uint8_t MPU6050::dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy); 535 | // uint8_t MPU6050::dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy); 536 | // uint8_t MPU6050::dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy); 537 | // uint8_t MPU6050::dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy); 538 | // uint8_t MPU6050::dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy); 539 | // uint8_t MPU6050::dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy); 540 | // uint8_t MPU6050::dmpSendPacketNumber(uint_fast16_t accuracy); 541 | // uint8_t MPU6050::dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy); 542 | // uint8_t MPU6050::dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy); 543 | 544 | uint8_t MPU6050::dmpGetAccel(int32_t *data, const uint8_t* packet) { 545 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) 546 | if (packet == 0) packet = dmpPacketBuffer; 547 | data[0] = ((packet[28] << 24) + (packet[29] << 16) + (packet[30] << 8) + packet[31]); 548 | data[1] = ((packet[32] << 24) + (packet[33] << 16) + (packet[34] << 8) + packet[35]); 549 | data[2] = ((packet[36] << 24) + (packet[37] << 16) + (packet[38] << 8) + packet[39]); 550 | return 0; 551 | } 552 | uint8_t MPU6050::dmpGetAccel(int16_t *data, const uint8_t* packet) { 553 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) 554 | if (packet == 0) packet = dmpPacketBuffer; 555 | data[0] = (packet[28] << 8) + packet[29]; 556 | data[1] = (packet[32] << 8) + packet[33]; 557 | data[2] = (packet[36] << 8) + packet[37]; 558 | return 0; 559 | } 560 | uint8_t MPU6050::dmpGetAccel(VectorInt16 *v, const uint8_t* packet) { 561 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) 562 | if (packet == 0) packet = dmpPacketBuffer; 563 | v -> x = (packet[28] << 8) + packet[29]; 564 | v -> y = (packet[32] << 8) + packet[33]; 565 | v -> z = (packet[36] << 8) + packet[37]; 566 | return 0; 567 | } 568 | uint8_t MPU6050::dmpGetQuaternion(int32_t *data, const uint8_t* packet) { 569 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) 570 | if (packet == 0) packet = dmpPacketBuffer; 571 | data[0] = ((packet[0] << 24) + (packet[1] << 16) + (packet[2] << 8) + packet[3]); 572 | data[1] = ((packet[4] << 24) + (packet[5] << 16) + (packet[6] << 8) + packet[7]); 573 | data[2] = ((packet[8] << 24) + (packet[9] << 16) + (packet[10] << 8) + packet[11]); 574 | data[3] = ((packet[12] << 24) + (packet[13] << 16) + (packet[14] << 8) + packet[15]); 575 | return 0; 576 | } 577 | uint8_t MPU6050::dmpGetQuaternion(int16_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[0] << 8) + packet[1]); 581 | data[1] = ((packet[4] << 8) + packet[5]); 582 | data[2] = ((packet[8] << 8) + packet[9]); 583 | data[3] = ((packet[12] << 8) + packet[13]); 584 | return 0; 585 | } 586 | uint8_t MPU6050::dmpGetQuaternion(Quaternion *q, const uint8_t* packet) { 587 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) 588 | int16_t qI[4]; 589 | uint8_t status = dmpGetQuaternion(qI, packet); 590 | if (status == 0) { 591 | q -> w = (float)qI[0] / 16384.0f; 592 | q -> x = (float)qI[1] / 16384.0f; 593 | q -> y = (float)qI[2] / 16384.0f; 594 | q -> z = (float)qI[3] / 16384.0f; 595 | return 0; 596 | } 597 | return status; // int16 return value, indicates error if this line is reached 598 | } 599 | // uint8_t MPU6050::dmpGet6AxisQuaternion(long *data, const uint8_t* packet); 600 | // uint8_t MPU6050::dmpGetRelativeQuaternion(long *data, const uint8_t* packet); 601 | uint8_t MPU6050::dmpGetGyro(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[16] << 24) + (packet[17] << 16) + (packet[18] << 8) + packet[19]); 605 | data[1] = ((packet[20] << 24) + (packet[21] << 16) + (packet[22] << 8) + packet[23]); 606 | data[2] = ((packet[24] << 24) + (packet[25] << 16) + (packet[26] << 8) + packet[27]); 607 | return 0; 608 | } 609 | uint8_t MPU6050::dmpGetGyro(int16_t *data, const uint8_t* packet) { 610 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) 611 | if (packet == 0) packet = dmpPacketBuffer; 612 | data[0] = (packet[16] << 8) + packet[17]; 613 | data[1] = (packet[20] << 8) + packet[21]; 614 | data[2] = (packet[24] << 8) + packet[25]; 615 | return 0; 616 | } 617 | // uint8_t MPU6050::dmpSetLinearAccelFilterCoefficient(float coef); 618 | // uint8_t MPU6050::dmpGetLinearAccel(long *data, const uint8_t* packet); 619 | uint8_t MPU6050::dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity) { 620 | // get rid of the gravity component (+1g = +4096 in standard DMP FIFO packet) 621 | v -> x = vRaw -> x - gravity -> x*4096; 622 | v -> y = vRaw -> y - gravity -> y*4096; 623 | v -> z = vRaw -> z - gravity -> z*4096; 624 | return 0; 625 | } 626 | // uint8_t MPU6050::dmpGetLinearAccelInWorld(long *data, const uint8_t* packet); 627 | uint8_t MPU6050::dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q) { 628 | // rotate measured 3D acceleration vector into original state 629 | // frame of reference based on orientation quaternion 630 | memcpy(v, vReal, sizeof(VectorInt16)); 631 | v -> rotate(q); 632 | return 0; 633 | } 634 | // uint8_t MPU6050::dmpGetGyroAndAccelSensor(long *data, const uint8_t* packet); 635 | // uint8_t MPU6050::dmpGetGyroSensor(long *data, const uint8_t* packet); 636 | // uint8_t MPU6050::dmpGetControlData(long *data, const uint8_t* packet); 637 | // uint8_t MPU6050::dmpGetTemperature(long *data, const uint8_t* packet); 638 | // uint8_t MPU6050::dmpGetGravity(long *data, const uint8_t* packet); 639 | uint8_t MPU6050::dmpGetGravity(VectorFloat *v, Quaternion *q) { 640 | v -> x = 2 * (q -> x*q -> z - q -> w*q -> y); 641 | v -> y = 2 * (q -> w*q -> x + q -> y*q -> z); 642 | v -> z = q -> w*q -> w - q -> x*q -> x - q -> y*q -> y + q -> z*q -> z; 643 | return 0; 644 | } 645 | // uint8_t MPU6050::dmpGetUnquantizedAccel(long *data, const uint8_t* packet); 646 | // uint8_t MPU6050::dmpGetQuantizedAccel(long *data, const uint8_t* packet); 647 | // uint8_t MPU6050::dmpGetExternalSensorData(long *data, int size, const uint8_t* packet); 648 | // uint8_t MPU6050::dmpGetEIS(long *data, const uint8_t* packet); 649 | 650 | uint8_t MPU6050::dmpGetEuler(float *data, Quaternion *q) { 651 | 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 652 | data[1] = -asin(2*q -> x*q -> z + 2*q -> w*q -> y); // theta 653 | 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 654 | return 0; 655 | } 656 | uint8_t MPU6050::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) { 657 | // yaw: (about Z axis) 658 | data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1); 659 | // pitch: (nose up/down, about Y axis) 660 | data[1] = atan(gravity -> x / sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z)); 661 | // roll: (tilt left/right, about X axis) 662 | data[2] = atan(gravity -> y / sqrt(gravity -> x*gravity -> x + gravity -> z*gravity -> z)); 663 | return 0; 664 | } 665 | 666 | // uint8_t MPU6050::dmpGetAccelFloat(float *data, const uint8_t* packet); 667 | // uint8_t MPU6050::dmpGetQuaternionFloat(float *data, const uint8_t* packet); 668 | 669 | uint8_t MPU6050::dmpProcessFIFOPacket(const unsigned char *dmpData) { 670 | /*for (uint8_t k = 0; k < dmpPacketSize; k++) { 671 | if (dmpData[k] < 0x10) Serial.print("0"); 672 | Serial.print(dmpData[k], HEX); 673 | Serial.print(" "); 674 | } 675 | Serial.print("\n");*/ 676 | //Serial.println((uint16_t)dmpPacketBuffer); 677 | return 0; 678 | } 679 | uint8_t MPU6050::dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed) { 680 | uint8_t status; 681 | uint8_t buf[dmpPacketSize]; 682 | for (uint8_t i = 0; i < numPackets; i++) { 683 | // read packet from FIFO 684 | getFIFOBytes(buf, dmpPacketSize); 685 | 686 | // process packet 687 | if ((status = dmpProcessFIFOPacket(buf)) > 0) return status; 688 | 689 | // increment external process count variable, if supplied 690 | if (processed != 0) *processed++; 691 | } 692 | return 0; 693 | } 694 | 695 | // uint8_t MPU6050::dmpSetFIFOProcessedCallback(void (*func) (void)); 696 | 697 | // uint8_t MPU6050::dmpInitFIFOParam(); 698 | // uint8_t MPU6050::dmpCloseFIFO(); 699 | // uint8_t MPU6050::dmpSetGyroDataSource(uint_fast8_t source); 700 | // uint8_t MPU6050::dmpDecodeQuantizedAccel(); 701 | // uint32_t MPU6050::dmpGetGyroSumOfSquare(); 702 | // uint32_t MPU6050::dmpGetAccelSumOfSquare(); 703 | // void MPU6050::dmpOverrideQuaternion(long *q); 704 | uint16_t MPU6050::dmpGetFIFOPacketSize() { 705 | return dmpPacketSize; 706 | } 707 | 708 | #endif /* _MPU6050_6AXIS_MOTIONAPPS20_H_ */ 709 | -------------------------------------------------------------------------------- /sensors/MPU/dmpMPU9150.h: -------------------------------------------------------------------------------- 1 | // I2Cdev library collection - MPU9150 I2C device class, 9-axis MotionApps 4.1 implementation 2 | // Based on InvenSense MPU-6050 register map document rev. 2.0, 5/19/2011 (RM-MPU-6000A-00) 3 | // 6/18/2012 by Jeff Rowberg 4 | // Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib 5 | // 6 | // Changelog: 7 | // ... - ongoing debug release 8 | 9 | /* ============================================ 10 | I2Cdev device library code is placed under the MIT license 11 | Copyright (c) 2012 Jeff Rowberg 12 | 13 | Permission is hereby granted, free of charge, to any person obtaining a copy 14 | of this software and associated documentation files (the "Software"), to deal 15 | in the Software without restriction, including without limitation the rights 16 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 17 | copies of the Software, and to permit persons to whom the Software is 18 | furnished to do so, subject to the following conditions: 19 | 20 | The above copyright notice and this permission notice shall be included in 21 | all copies or substantial portions of the Software. 22 | 23 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 24 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 25 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 26 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 27 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 28 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 29 | THE SOFTWARE. 30 | =============================================== 31 | */ 32 | 33 | #ifndef _DMPMPU9150_H_ 34 | #define _DMPMPU9150_H_ 35 | 36 | #include "I2Cdev.h" 37 | #include "helper_3dmath.h" 38 | 39 | // MotionApps 4.1 DMP implementation, built using the MPU-9150 "MotionFit" board 40 | #define MPU9150_INCLUDE_DMP9150 41 | 42 | #include "sensors/MPU/MPU9150.h" 43 | 44 | #if defined(__AVR__) 45 | #include 46 | #define fontdatatype uint8_t 47 | #elif defined(__PIC32MX__) 48 | #define PROGMEM 49 | #define fontdatatype const unsigned char 50 | #elif defined(__arm__) 51 | #define PROGMEM 52 | #define fontdatatype const unsigned char 53 | #endif 54 | 55 | // NOTE! Enabling DEBUG adds about 3.3kB to the flash program size. 56 | // Debug output is now working even on ATMega328P MCUs (e.g. Arduino Uno) 57 | // after moving string constants to flash memory storage using the F() 58 | // compiler macro (Arduino IDE 1.0+ required). 59 | 60 | // #define DEBUG 61 | #ifdef DEBUG 62 | #define DEBUG_PRINT(x) Serial.print(x) 63 | #define DEBUG_PRINTF(x, y) Serial.print(x, y) 64 | #define DEBUG_PRINTLN(x) Serial.println(x) 65 | #define DEBUG_PRINTLNF(x, y) Serial.println(x, y) 66 | #else 67 | #define DEBUG_PRINT(x) 68 | #define DEBUG_PRINTF(x, y) 69 | #define DEBUG_PRINTLN(x) 70 | #define DEBUG_PRINTLNF(x, y) 71 | #endif 72 | 73 | #define MPU9150_DMP_CODE_SIZE 1962 // dmpMemory[] 74 | #define MPU9150_DMP_CONFIG_SIZE 232 // dmpConfig[] 75 | #define MPU9150_DMP_UPDATES_SIZE 140 // dmpUpdates[] 76 | 77 | /* ================================================================================================ * 78 | | Default MotionApps v4.1 48-byte FIFO packet structure: | 79 | | | 80 | | [QUAT W][ ][QUAT X][ ][QUAT Y][ ][QUAT Z][ ][GYRO X][ ][GYRO Y][ ] | 81 | | 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 | 82 | | | 83 | | [GYRO Z][ ][MAG X ][MAG Y ][MAG Z ][ACC X ][ ][ACC Y ][ ][ACC Z ][ ][ ] | 84 | | 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 | 85 | * ================================================================================================ */ 86 | 87 | // this block of memory gets written to the MPU on start-up, and it seems 88 | // to be volatile memory, so it has to be done each time (it only takes ~1 89 | // second though) 90 | const unsigned char dmpMemory[MPU9150_DMP_CODE_SIZE] PROGMEM = { 91 | // bank 0, 256 bytes 92 | 0xFB, 0x00, 0x00, 0x3E, 0x00, 0x0B, 0x00, 0x36, 0x00, 0x01, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00, 93 | 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0xFA, 0x80, 0x00, 0x0B, 0x12, 0x82, 0x00, 0x01, 94 | 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 95 | 0x00, 0x28, 0x00, 0x00, 0xFF, 0xFF, 0x45, 0x81, 0xFF, 0xFF, 0xFA, 0x72, 0x00, 0x00, 0x00, 0x00, 96 | 0x00, 0x00, 0x03, 0xE8, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x7F, 0xFF, 0xFF, 0xFE, 0x80, 0x01, 97 | 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 98 | 0x00, 0x3E, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xCA, 0xE3, 0x09, 0x3E, 0x80, 0x00, 0x00, 99 | 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00, 100 | 0x41, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x0B, 0x2A, 0x00, 0x00, 0x16, 0x55, 0x00, 0x00, 0x21, 0x82, 101 | 0xFD, 0x87, 0x26, 0x50, 0xFD, 0x80, 0x00, 0x00, 0x00, 0x1F, 0x00, 0x00, 0x00, 0x05, 0x80, 0x00, 102 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 103 | 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x6F, 0x00, 0x02, 0x65, 0x32, 0x00, 0x00, 0x5E, 0xC0, 104 | 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 105 | 0xFB, 0x8C, 0x6F, 0x5D, 0xFD, 0x5D, 0x08, 0xD9, 0x00, 0x7C, 0x73, 0x3B, 0x00, 0x6C, 0x12, 0xCC, 106 | 0x32, 0x00, 0x13, 0x9D, 0x32, 0x00, 0xD0, 0xD6, 0x32, 0x00, 0x08, 0x00, 0x40, 0x00, 0x01, 0xF4, 107 | 0xFF, 0xE6, 0x80, 0x79, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0xD0, 0xD6, 0x00, 0x00, 0x27, 0x10, 108 | 109 | // bank 1, 256 bytes 110 | 0xFB, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 111 | 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x00, 112 | 0x00, 0x00, 0xFA, 0x36, 0xFF, 0xBC, 0x30, 0x8E, 0x00, 0x05, 0xFB, 0xF0, 0xFF, 0xD9, 0x5B, 0xC8, 113 | 0xFF, 0xD0, 0x9A, 0xBE, 0x00, 0x00, 0x10, 0xA9, 0xFF, 0xF4, 0x1E, 0xB2, 0x00, 0xCE, 0xBB, 0xF7, 114 | 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x04, 0x00, 0x02, 0x00, 0x02, 0x02, 0x00, 0x00, 0x0C, 115 | 0xFF, 0xC2, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0xCF, 0x80, 0x00, 0x40, 0x00, 0x00, 0x00, 116 | 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14, 117 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 118 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 119 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 120 | 0x00, 0x00, 0x00, 0x00, 0x03, 0x3F, 0x68, 0xB6, 0x79, 0x35, 0x28, 0xBC, 0xC6, 0x7E, 0xD1, 0x6C, 121 | 0x80, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0xB2, 0x6A, 0x00, 0x00, 0x00, 0x00, 122 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3F, 0xF0, 0x00, 0x00, 0x00, 0x30, 123 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 124 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 125 | 0x00, 0x00, 0x25, 0x4D, 0x00, 0x2F, 0x70, 0x6D, 0x00, 0x00, 0x05, 0xAE, 0x00, 0x0C, 0x02, 0xD0, 126 | 127 | // bank 2, 256 bytes 128 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 129 | 0x00, 0x00, 0x01, 0x00, 0x00, 0x44, 0x00, 0x00, 0x00, 0x00, 0x0C, 0x00, 0x00, 0x00, 0x01, 0x00, 130 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x54, 0x00, 0x00, 0xFF, 0xEF, 0x00, 0x00, 131 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 132 | 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 133 | 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 134 | 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 135 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 136 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 137 | 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 138 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 139 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 140 | 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 141 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 142 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x47, 0x78, 0xA2, 143 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 144 | 145 | // bank 3, 256 bytes 146 | 0xD8, 0xDC, 0xF4, 0xD8, 0xB9, 0xAB, 0xF3, 0xF8, 0xFA, 0xF1, 0xBA, 0xA2, 0xDE, 0xB2, 0xB8, 0xB4, 147 | 0xA8, 0x81, 0x98, 0xF7, 0x4A, 0x90, 0x7F, 0x91, 0x6A, 0xF3, 0xF9, 0xDB, 0xA8, 0xF9, 0xB0, 0xBA, 148 | 0xA0, 0x80, 0xF2, 0xCE, 0x81, 0xF3, 0xC2, 0xF1, 0xC1, 0xF2, 0xC3, 0xF3, 0xCC, 0xA2, 0xB2, 0x80, 149 | 0xF1, 0xC6, 0xD8, 0x80, 0xBA, 0xA7, 0xDF, 0xDF, 0xDF, 0xF2, 0xA7, 0xC3, 0xCB, 0xC5, 0xB6, 0xF0, 150 | 0x87, 0xA2, 0x94, 0x24, 0x48, 0x70, 0x3C, 0x95, 0x40, 0x68, 0x34, 0x58, 0x9B, 0x78, 0xA2, 0xF1, 151 | 0x83, 0x92, 0x2D, 0x55, 0x7D, 0xD8, 0xB1, 0xB4, 0xB8, 0xA1, 0xD0, 0x91, 0x80, 0xF2, 0x70, 0xF3, 152 | 0x70, 0xF2, 0x7C, 0x80, 0xA8, 0xF1, 0x01, 0xB0, 0x98, 0x87, 0xD9, 0x43, 0xD8, 0x86, 0xC9, 0x88, 153 | 0xBA, 0xA1, 0xF2, 0x0E, 0xB8, 0x97, 0x80, 0xF1, 0xA9, 0xDF, 0xDF, 0xDF, 0xAA, 0xDF, 0xDF, 0xDF, 154 | 0xF2, 0xAA, 0xC5, 0xCD, 0xC7, 0xA9, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97, 0x97, 0xF1, 0xA9, 0x89, 155 | 0x26, 0x46, 0x66, 0xB0, 0xB4, 0xBA, 0x80, 0xAC, 0xDE, 0xF2, 0xCA, 0xF1, 0xB2, 0x8C, 0x02, 0xA9, 156 | 0xB6, 0x98, 0x00, 0x89, 0x0E, 0x16, 0x1E, 0xB8, 0xA9, 0xB4, 0x99, 0x2C, 0x54, 0x7C, 0xB0, 0x8A, 157 | 0xA8, 0x96, 0x36, 0x56, 0x76, 0xF1, 0xB9, 0xAF, 0xB4, 0xB0, 0x83, 0xC0, 0xB8, 0xA8, 0x97, 0x11, 158 | 0xB1, 0x8F, 0x98, 0xB9, 0xAF, 0xF0, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xF1, 0xA3, 0x29, 0x55, 159 | 0x7D, 0xAF, 0x83, 0xB5, 0x93, 0xF0, 0x00, 0x28, 0x50, 0xF5, 0xBA, 0xAD, 0x8F, 0x9F, 0x28, 0x54, 160 | 0x7C, 0xB9, 0xF1, 0xA3, 0x86, 0x9F, 0x61, 0xA6, 0xDA, 0xDE, 0xDF, 0xDB, 0xB2, 0xB6, 0x8E, 0x9D, 161 | 0xAE, 0xF5, 0x60, 0x68, 0x70, 0xB1, 0xB5, 0xF1, 0xDA, 0xA6, 0xDF, 0xD9, 0xA6, 0xFA, 0xA3, 0x86, 162 | 163 | // bank 4, 256 bytes 164 | 0x96, 0xDB, 0x31, 0xA6, 0xD9, 0xF8, 0xDF, 0xBA, 0xA6, 0x8F, 0xC2, 0xC5, 0xC7, 0xB2, 0x8C, 0xC1, 165 | 0xB8, 0xA2, 0xDF, 0xDF, 0xDF, 0xA3, 0xDF, 0xDF, 0xDF, 0xD8, 0xD8, 0xF1, 0xB8, 0xA8, 0xB2, 0x86, 166 | 0xB4, 0x98, 0x0D, 0x35, 0x5D, 0xB8, 0xAA, 0x98, 0xB0, 0x87, 0x2D, 0x35, 0x3D, 0xB2, 0xB6, 0xBA, 167 | 0xAF, 0x8C, 0x96, 0x19, 0x8F, 0x9F, 0xA7, 0x0E, 0x16, 0x1E, 0xB4, 0x9A, 0xB8, 0xAA, 0x87, 0x2C, 168 | 0x54, 0x7C, 0xB9, 0xA3, 0xDE, 0xDF, 0xDF, 0xA3, 0xB1, 0x80, 0xF2, 0xC4, 0xCD, 0xC9, 0xF1, 0xB8, 169 | 0xA9, 0xB4, 0x99, 0x83, 0x0D, 0x35, 0x5D, 0x89, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0xB5, 0x93, 0xA3, 170 | 0x0E, 0x16, 0x1E, 0xA9, 0x2C, 0x54, 0x7C, 0xB8, 0xB4, 0xB0, 0xF1, 0x97, 0x83, 0xA8, 0x11, 0x84, 171 | 0xA5, 0x09, 0x98, 0xA3, 0x83, 0xF0, 0xDA, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xD8, 0xF1, 0xA5, 172 | 0x29, 0x55, 0x7D, 0xA5, 0x85, 0x95, 0x02, 0x1A, 0x2E, 0x3A, 0x56, 0x5A, 0x40, 0x48, 0xF9, 0xF3, 173 | 0xA3, 0xD9, 0xF8, 0xF0, 0x98, 0x83, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0x97, 0x82, 0xA8, 0xF1, 174 | 0x11, 0xF0, 0x98, 0xA2, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xDA, 0xF3, 0xDE, 0xD8, 0x83, 0xA5, 175 | 0x94, 0x01, 0xD9, 0xA3, 0x02, 0xF1, 0xA2, 0xC3, 0xC5, 0xC7, 0xD8, 0xF1, 0x84, 0x92, 0xA2, 0x4D, 176 | 0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9, 177 | 0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0x93, 0xA3, 0x4D, 178 | 0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9, 179 | 0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0xA8, 0x8A, 0x9A, 180 | 181 | // bank 5, 256 bytes 182 | 0xF0, 0x28, 0x50, 0x78, 0x9E, 0xF3, 0x88, 0x18, 0xF1, 0x9F, 0x1D, 0x98, 0xA8, 0xD9, 0x08, 0xD8, 183 | 0xC8, 0x9F, 0x12, 0x9E, 0xF3, 0x15, 0xA8, 0xDA, 0x12, 0x10, 0xD8, 0xF1, 0xAF, 0xC8, 0x97, 0x87, 184 | 0x34, 0xB5, 0xB9, 0x94, 0xA4, 0x21, 0xF3, 0xD9, 0x22, 0xD8, 0xF2, 0x2D, 0xF3, 0xD9, 0x2A, 0xD8, 185 | 0xF2, 0x35, 0xF3, 0xD9, 0x32, 0xD8, 0x81, 0xA4, 0x60, 0x60, 0x61, 0xD9, 0x61, 0xD8, 0x6C, 0x68, 186 | 0x69, 0xD9, 0x69, 0xD8, 0x74, 0x70, 0x71, 0xD9, 0x71, 0xD8, 0xB1, 0xA3, 0x84, 0x19, 0x3D, 0x5D, 187 | 0xA3, 0x83, 0x1A, 0x3E, 0x5E, 0x93, 0x10, 0x30, 0x81, 0x10, 0x11, 0xB8, 0xB0, 0xAF, 0x8F, 0x94, 188 | 0xF2, 0xDA, 0x3E, 0xD8, 0xB4, 0x9A, 0xA8, 0x87, 0x29, 0xDA, 0xF8, 0xD8, 0x87, 0x9A, 0x35, 0xDA, 189 | 0xF8, 0xD8, 0x87, 0x9A, 0x3D, 0xDA, 0xF8, 0xD8, 0xB1, 0xB9, 0xA4, 0x98, 0x85, 0x02, 0x2E, 0x56, 190 | 0xA5, 0x81, 0x00, 0x0C, 0x14, 0xA3, 0x97, 0xB0, 0x8A, 0xF1, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9, 191 | 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x84, 0x0D, 0xDA, 0x0E, 0xD8, 0xA3, 0x29, 0x83, 0xDA, 192 | 0x2C, 0x0E, 0xD8, 0xA3, 0x84, 0x49, 0x83, 0xDA, 0x2C, 0x4C, 0x0E, 0xD8, 0xB8, 0xB0, 0x97, 0x86, 193 | 0xA8, 0x31, 0x9B, 0x06, 0x99, 0x07, 0xAB, 0x97, 0x28, 0x88, 0x9B, 0xF0, 0x0C, 0x20, 0x14, 0x40, 194 | 0xB9, 0xA3, 0x8A, 0xC3, 0xC5, 0xC7, 0x9A, 0xA3, 0x28, 0x50, 0x78, 0xF1, 0xB5, 0x93, 0x01, 0xD9, 195 | 0xDF, 0xDF, 0xDF, 0xD8, 0xB8, 0xB4, 0xA8, 0x8C, 0x9C, 0xF0, 0x04, 0x28, 0x51, 0x79, 0x1D, 0x30, 196 | 0x14, 0x38, 0xB2, 0x82, 0xAB, 0xD0, 0x98, 0x2C, 0x50, 0x50, 0x78, 0x78, 0x9B, 0xF1, 0x1A, 0xB0, 197 | 0xF0, 0xB1, 0x83, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0xB0, 0x8B, 0x29, 0x51, 0x79, 0xB1, 0x83, 0x24, 198 | 199 | // bank 6, 256 bytes 200 | 0x70, 0x59, 0xB0, 0x8B, 0x20, 0x58, 0x71, 0xB1, 0x83, 0x44, 0x69, 0x38, 0xB0, 0x8B, 0x39, 0x40, 201 | 0x68, 0xB1, 0x83, 0x64, 0x48, 0x31, 0xB0, 0x8B, 0x30, 0x49, 0x60, 0xA5, 0x88, 0x20, 0x09, 0x71, 202 | 0x58, 0x44, 0x68, 0x11, 0x39, 0x64, 0x49, 0x30, 0x19, 0xF1, 0xAC, 0x00, 0x2C, 0x54, 0x7C, 0xF0, 203 | 0x8C, 0xA8, 0x04, 0x28, 0x50, 0x78, 0xF1, 0x88, 0x97, 0x26, 0xA8, 0x59, 0x98, 0xAC, 0x8C, 0x02, 204 | 0x26, 0x46, 0x66, 0xF0, 0x89, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 205 | 0x64, 0x48, 0x31, 0xA9, 0x88, 0x09, 0x20, 0x59, 0x70, 0xAB, 0x11, 0x38, 0x40, 0x69, 0xA8, 0x19, 206 | 0x31, 0x48, 0x60, 0x8C, 0xA8, 0x3C, 0x41, 0x5C, 0x20, 0x7C, 0x00, 0xF1, 0x87, 0x98, 0x19, 0x86, 207 | 0xA8, 0x6E, 0x76, 0x7E, 0xA9, 0x99, 0x88, 0x2D, 0x55, 0x7D, 0x9E, 0xB9, 0xA3, 0x8A, 0x22, 0x8A, 208 | 0x6E, 0x8A, 0x56, 0x8A, 0x5E, 0x9F, 0xB1, 0x83, 0x06, 0x26, 0x46, 0x66, 0x0E, 0x2E, 0x4E, 0x6E, 209 | 0x9D, 0xB8, 0xAD, 0x00, 0x2C, 0x54, 0x7C, 0xF2, 0xB1, 0x8C, 0xB4, 0x99, 0xB9, 0xA3, 0x2D, 0x55, 210 | 0x7D, 0x81, 0x91, 0xAC, 0x38, 0xAD, 0x3A, 0xB5, 0x83, 0x91, 0xAC, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 211 | 0xD9, 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0x8C, 0x9D, 0xAE, 0x29, 0xD9, 0x04, 0xAE, 0xD8, 0x51, 212 | 0xD9, 0x04, 0xAE, 0xD8, 0x79, 0xD9, 0x04, 0xD8, 0x81, 0xF3, 0x9D, 0xAD, 0x00, 0x8D, 0xAE, 0x19, 213 | 0x81, 0xAD, 0xD9, 0x01, 0xD8, 0xF2, 0xAE, 0xDA, 0x26, 0xD8, 0x8E, 0x91, 0x29, 0x83, 0xA7, 0xD9, 214 | 0xAD, 0xAD, 0xAD, 0xAD, 0xF3, 0x2A, 0xD8, 0xD8, 0xF1, 0xB0, 0xAC, 0x89, 0x91, 0x3E, 0x5E, 0x76, 215 | 0xF3, 0xAC, 0x2E, 0x2E, 0xF1, 0xB1, 0x8C, 0x5A, 0x9C, 0xAC, 0x2C, 0x28, 0x28, 0x28, 0x9C, 0xAC, 216 | 217 | // bank 7, 170 bytes (remainder) 218 | 0x30, 0x18, 0xA8, 0x98, 0x81, 0x28, 0x34, 0x3C, 0x97, 0x24, 0xA7, 0x28, 0x34, 0x3C, 0x9C, 0x24, 219 | 0xF2, 0xB0, 0x89, 0xAC, 0x91, 0x2C, 0x4C, 0x6C, 0x8A, 0x9B, 0x2D, 0xD9, 0xD8, 0xD8, 0x51, 0xD9, 220 | 0xD8, 0xD8, 0x79, 0xD9, 0xD8, 0xD8, 0xF1, 0x9E, 0x88, 0xA3, 0x31, 0xDA, 0xD8, 0xD8, 0x91, 0x2D, 221 | 0xD9, 0x28, 0xD8, 0x4D, 0xD9, 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x83, 0x93, 0x35, 0x3D, 222 | 0x80, 0x25, 0xDA, 0xD8, 0xD8, 0x85, 0x69, 0xDA, 0xD8, 0xD8, 0xB4, 0x93, 0x81, 0xA3, 0x28, 0x34, 223 | 0x3C, 0xF3, 0xAB, 0x8B, 0xA3, 0x91, 0xB6, 0x09, 0xB4, 0xD9, 0xAB, 0xDE, 0xB0, 0x87, 0x9C, 0xB9, 224 | 0xA3, 0xDD, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x95, 0xF1, 0xA3, 0xA3, 0xA3, 0x9D, 0xF1, 0xA3, 0xA3, 225 | 0xA3, 0xA3, 0xF2, 0xA3, 0xB4, 0x90, 0x80, 0xF2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 226 | 0xA3, 0xA3, 0xB2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xB0, 0x87, 0xB5, 0x99, 0xF1, 0xA3, 0xA3, 227 | 0xA3, 0x98, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x97, 0xA3, 0xA3, 0xA3, 0xA3, 0xF3, 0x9B, 0xA3, 0xA3, 228 | 0xDC, 0xB9, 0xA7, 0xF1, 0x26, 0x26, 0x26, 0xD8, 0xD8, 0xFF 229 | }; 230 | 231 | const unsigned char dmpConfig[MPU9150_DMP_CONFIG_SIZE] PROGMEM = { 232 | // BANK OFFSET LENGTH [DATA] 233 | 0x02, 0xEC, 0x04, 0x00, 0x47, 0x7D, 0x1A, // ? 234 | 0x03, 0x82, 0x03, 0x4C, 0xCD, 0x6C, // FCFG_1 inv_set_gyro_calibration 235 | 0x03, 0xB2, 0x03, 0x36, 0x56, 0x76, // FCFG_3 inv_set_gyro_calibration 236 | 0x00, 0x68, 0x04, 0x02, 0xCA, 0xE3, 0x09, // D_0_104 inv_set_gyro_calibration 237 | 0x01, 0x0C, 0x04, 0x00, 0x00, 0x00, 0x00, // D_1_152 inv_set_accel_calibration 238 | 0x03, 0x86, 0x03, 0x0C, 0xC9, 0x2C, // FCFG_2 inv_set_accel_calibration 239 | 0x03, 0x90, 0x03, 0x26, 0x46, 0x66, // (continued)...FCFG_2 inv_set_accel_calibration 240 | 0x00, 0x6C, 0x02, 0x40, 0x00, // D_0_108 inv_set_accel_calibration 241 | 242 | 0x02, 0x40, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_00 inv_set_compass_calibration 243 | 0x02, 0x44, 0x04, 0x40, 0x00, 0x00, 0x00, // CPASS_MTX_01 244 | 0x02, 0x48, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_02 245 | 0x02, 0x4C, 0x04, 0x40, 0x00, 0x00, 0x00, // CPASS_MTX_10 246 | 0x02, 0x50, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_11 247 | 0x02, 0x54, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_12 248 | 0x02, 0x58, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_20 249 | 0x02, 0x5C, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_21 250 | 0x02, 0xBC, 0x04, 0xC0, 0x00, 0x00, 0x00, // CPASS_MTX_22 251 | 252 | 0x01, 0xEC, 0x04, 0x00, 0x00, 0x40, 0x00, // D_1_236 inv_apply_endian_accel 253 | 0x03, 0x86, 0x06, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97, // FCFG_2 inv_set_mpu_sensors 254 | 0x04, 0x22, 0x03, 0x0D, 0x35, 0x5D, // CFG_MOTION_BIAS inv_turn_on_bias_from_no_motion 255 | 0x00, 0xA3, 0x01, 0x00, // ? 256 | 0x04, 0x29, 0x04, 0x87, 0x2D, 0x35, 0x3D, // FCFG_5 inv_set_bias_update 257 | 0x07, 0x62, 0x05, 0xF1, 0x20, 0x28, 0x30, 0x38, // CFG_8 inv_send_quaternion 258 | 0x07, 0x9F, 0x01, 0x30, // CFG_16 inv_set_footer 259 | 0x07, 0x67, 0x01, 0x9A, // CFG_GYRO_SOURCE inv_send_gyro 260 | 0x07, 0x68, 0x04, 0xF1, 0x28, 0x30, 0x38, // CFG_9 inv_send_gyro -> inv_construct3_fifo 261 | 0x07, 0x62, 0x05, 0xF1, 0x20, 0x28, 0x30, 0x38, // ? 262 | 0x02, 0x0C, 0x04, 0x00, 0x00, 0x00, 0x00, // ? 263 | 0x07, 0x83, 0x06, 0xC2, 0xCA, 0xC4, 0xA3, 0xA3, 0xA3, // ? 264 | // SPECIAL 0x01 = enable interrupts 265 | 0x00, 0x00, 0x00, 0x01, // SET INT_ENABLE, SPECIAL INSTRUCTION 266 | 0x07, 0xA7, 0x01, 0xFE, // ? 267 | 0x07, 0x62, 0x05, 0xF1, 0x20, 0x28, 0x30, 0x38, // ? 268 | 0x07, 0x67, 0x01, 0x9A, // ? 269 | 0x07, 0x68, 0x04, 0xF1, 0x28, 0x30, 0x38, // CFG_12 inv_send_accel -> inv_construct3_fifo 270 | 0x07, 0x8D, 0x04, 0xF1, 0x28, 0x30, 0x38, // ??? CFG_12 inv_send_mag -> inv_construct3_fifo 271 | 0x02, 0x16, 0x02, 0x00, 0x03 // D_0_22 inv_set_fifo_rate 272 | 273 | // This very last 0x01 WAS a 0x09, which drops the FIFO rate down to 20 Hz. 0x07 is 25 Hz, 274 | // 0x01 is 100Hz. Going faster than 100Hz (0x00=200Hz) tends to result in very noisy data. 275 | // DMP output frequency is calculated easily using this equation: (200Hz / (1 + value)) 276 | 277 | // It is important to make sure the host processor can keep up with reading and processing 278 | // the FIFO output at the desired rate. Handling FIFO overflow cleanly is also a good idea. 279 | }; 280 | 281 | const unsigned char dmpUpdates[MPU9150_DMP_UPDATES_SIZE] PROGMEM = { 282 | 0x01, 0xB2, 0x02, 0xFF, 0xF5, 283 | 0x01, 0x90, 0x04, 0x0A, 0x0D, 0x97, 0xC0, 284 | 0x00, 0xA3, 0x01, 0x00, 285 | 0x04, 0x29, 0x04, 0x87, 0x2D, 0x35, 0x3D, 286 | 0x01, 0x6A, 0x02, 0x06, 0x00, 287 | 0x01, 0x60, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 288 | 0x00, 0x60, 0x04, 0x40, 0x00, 0x00, 0x00, 289 | 0x02, 0x60, 0x0C, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 290 | 0x01, 0x08, 0x02, 0x01, 0x20, 291 | 0x01, 0x0A, 0x02, 0x00, 0x4E, 292 | 0x01, 0x02, 0x02, 0xFE, 0xB3, 293 | 0x02, 0x6C, 0x04, 0x00, 0x00, 0x00, 0x00, // READ 294 | 0x02, 0x6C, 0x04, 0xFA, 0xFE, 0x00, 0x00, 295 | 0x02, 0x60, 0x0C, 0xFF, 0xFF, 0xCB, 0x4D, 0x00, 0x01, 0x08, 0xC1, 0xFF, 0xFF, 0xBC, 0x2C, 296 | 0x02, 0xF4, 0x04, 0x00, 0x00, 0x00, 0x00, 297 | 0x02, 0xF8, 0x04, 0x00, 0x00, 0x00, 0x00, 298 | 0x02, 0xFC, 0x04, 0x00, 0x00, 0x00, 0x00, 299 | 0x00, 0x60, 0x04, 0x40, 0x00, 0x00, 0x00, 300 | 0x00, 0x60, 0x04, 0x00, 0x40, 0x00, 0x00 301 | }; 302 | 303 | uint8_t MPU9150::dmpInitialize() { 304 | // reset device 305 | DEBUG_PRINTLN(("\n\nResetting MPU9150...")); 306 | reset(); 307 | delay(30); // wait after reset 308 | 309 | // disable sleep mode 310 | DEBUG_PRINTLN(("Disabling sleep mode...")); 311 | setSleepEnabled(false); 312 | 313 | // get MPU product ID 314 | // DEBUG_PRINTLN(("Getting product ID...")); 315 | // uint8_t productID = getProductID(); 316 | // DEBUG_PRINT(("Product ID = ")); 317 | // DEBUG_PRINT(productID); 318 | 319 | // get MPU hardware revision 320 | DEBUG_PRINTLN(("Selecting user bank 16...")); 321 | setMemoryBank(0x10, true, true); 322 | DEBUG_PRINTLN(("Selecting memory byte 6...")); 323 | setMemoryStartAddress(0x06); 324 | DEBUG_PRINTLN(("Checking hardware revision...")); 325 | uint8_t hwRevision = readMemoryByte(); 326 | DEBUG_PRINT(("Revision @ user[16][6] = ")); 327 | DEBUG_PRINTLNF(hwRevision, HEX); 328 | DEBUG_PRINTLN(("Resetting memory bank selection to 0...")); 329 | setMemoryBank(0, false, false); 330 | 331 | // check OTP bank valid 332 | DEBUG_PRINTLN(("Reading OTP bank valid flag...")); 333 | uint8_t otpValid = getOTPBankValid(); 334 | DEBUG_PRINT(("OTP bank is ")); 335 | DEBUG_PRINTLN(otpValid ? ("valid!") : ("invalid!")); 336 | 337 | // get X/Y/Z gyro offsets 338 | DEBUG_PRINTLN(("Reading gyro offset values...")); 339 | int8_t xgOffset = getXGyroOffset(); 340 | int8_t ygOffset = getYGyroOffset(); 341 | int8_t zgOffset = getZGyroOffset(); 342 | DEBUG_PRINT(("X gyro offset = ")); 343 | DEBUG_PRINTLN(xgOffset); 344 | DEBUG_PRINT(("Y gyro offset = ")); 345 | DEBUG_PRINTLN(ygOffset); 346 | DEBUG_PRINT(("Z gyro offset = ")); 347 | DEBUG_PRINTLN(zgOffset); 348 | 349 | I2Cdev::readByte(devAddr, MPU9150_RA_USER_CTRL, buffer); // ? 350 | 351 | DEBUG_PRINTLN(("Enabling interrupt latch, clear on any read, AUX bypass enabled")); 352 | I2Cdev::writeByte(devAddr, MPU9150_RA_INT_PIN_CFG, 0x32); 353 | 354 | // enable MPU AUX I2C bypass mode 355 | //DEBUG_PRINTLN(("Enabling AUX I2C bypass mode...")); 356 | //setI2CBypassEnabled(true); 357 | 358 | DEBUG_PRINTLN(("Setting magnetometer mode to power-down...")); 359 | //mag -> setMode(0); 360 | I2Cdev::writeByte(0x0E, 0x0A, 0x00); 361 | 362 | DEBUG_PRINTLN(("Setting magnetometer mode to fuse access...")); 363 | //mag -> setMode(0x0F); 364 | I2Cdev::writeByte(0x0E, 0x0A, 0x0F); 365 | 366 | DEBUG_PRINTLN(("Reading mag magnetometer factory calibration...")); 367 | int8_t asax, asay, asaz; 368 | //mag -> getAdjustment(&asax, &asay, &asaz); 369 | I2Cdev::readBytes(0x0E, 0x10, 3, buffer); 370 | asax = (int8_t)buffer[0]; 371 | asay = (int8_t)buffer[1]; 372 | asaz = (int8_t)buffer[2]; 373 | DEBUG_PRINT(("Adjustment X/Y/Z = ")); 374 | DEBUG_PRINT(asax); 375 | DEBUG_PRINT((" / ")); 376 | DEBUG_PRINT(asay); 377 | DEBUG_PRINT((" / ")); 378 | DEBUG_PRINTLN(asaz); 379 | 380 | DEBUG_PRINTLN(("Setting magnetometer mode to power-down...")); 381 | //mag -> setMode(0); 382 | I2Cdev::writeByte(0x0E, 0x0A, 0x00); 383 | 384 | // load DMP code into memory banks 385 | DEBUG_PRINT(("Writing DMP code to MPU memory banks (")); 386 | DEBUG_PRINT(MPU9150_DMP_CODE_SIZE); 387 | DEBUG_PRINTLN((" bytes)")); 388 | if (writeProgMemoryBlock(dmpMemory, MPU9150_DMP_CODE_SIZE)) { 389 | DEBUG_PRINTLN(("Success! DMP code written and verified.")); 390 | 391 | DEBUG_PRINTLN(("Configuring DMP and related settings...")); 392 | 393 | // write DMP configuration 394 | DEBUG_PRINT(("Writing DMP configuration to MPU memory banks (")); 395 | DEBUG_PRINT(MPU9150_DMP_CONFIG_SIZE); 396 | DEBUG_PRINTLN((" bytes in config def)")); 397 | if (writeProgDMPConfigurationSet(dmpConfig, MPU9150_DMP_CONFIG_SIZE)) { 398 | DEBUG_PRINTLN(("Success! DMP configuration written and verified.")); 399 | 400 | DEBUG_PRINTLN(("Setting DMP and FIFO_OFLOW interrupts enabled...")); 401 | setIntEnabled(0x12); 402 | // setIntEnabled(0x82); // Interrupts for: DMP AND Free Fall 403 | 404 | DEBUG_PRINTLN(("Setting sample rate to 200Hz...")); 405 | setRate(4); // 1khz / (1 + 4) = 200 Hz 406 | 407 | DEBUG_PRINTLN(("Setting clock source to Z Gyro...")); 408 | setClockSource(MPU9150_CLOCK_PLL_ZGYRO); 409 | 410 | DEBUG_PRINTLN(("Setting DLPF bandwidth to 42Hz...")); 411 | setDLPFMode(MPU9150_DLPF_BW_42); 412 | 413 | DEBUG_PRINTLN(("Setting external frame sync to TEMP_OUT_L[0]...")); 414 | setExternalFrameSync(MPU9150_EXT_SYNC_TEMP_OUT_L); 415 | 416 | DEBUG_PRINTLN(("Setting gyro sensitivity to +/- 2000 deg/sec...")); 417 | setFullScaleGyroRange(MPU9150_GYRO_FS_2000); 418 | 419 | DEBUG_PRINTLN(("Setting DMP configuration bytes (function unknown)...")); 420 | setDMPConfig1(0x03); 421 | setDMPConfig2(0x00); 422 | 423 | DEBUG_PRINTLN(("Clearing OTP Bank flag...")); 424 | setOTPBankValid(false); 425 | 426 | DEBUG_PRINTLN(("Setting X/Y/Z gyro offsets to previous values...")); 427 | setXGyroOffset(xgOffset); 428 | setYGyroOffset(ygOffset); 429 | setZGyroOffset(zgOffset); 430 | 431 | DEBUG_PRINTLN(("Setting X/Y/Z gyro user offsets to zero...")); 432 | setXGyroOffsetUser(0); 433 | setYGyroOffsetUser(0); 434 | setZGyroOffsetUser(0); 435 | 436 | DEBUG_PRINTLN(("Writing final memory update 1/19 (function unknown)...")); 437 | uint8_t dmpUpdate[16], j; 438 | uint16_t pos = 0; 439 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = (dmpUpdates[pos]); 440 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 441 | 442 | DEBUG_PRINTLN(("Writing final memory update 2/19 (function unknown)...")); 443 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = (dmpUpdates[pos]); 444 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 445 | 446 | DEBUG_PRINTLN(("Resetting FIFO...")); 447 | resetFIFO(); 448 | 449 | DEBUG_PRINTLN(("Reading FIFO count...")); 450 | uint8_t fifoCount = getFIFOCount(); 451 | 452 | DEBUG_PRINT(("Current FIFO count=")); 453 | DEBUG_PRINTLN(fifoCount); 454 | uint8_t fifoBuffer[128]; 455 | //getFIFOBytes(fifoBuffer, fifoCount); 456 | 457 | DEBUG_PRINTLN(("Writing final memory update 3/19 (function unknown)...")); 458 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = (dmpUpdates[pos]); 459 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 460 | 461 | DEBUG_PRINTLN(("Writing final memory update 4/19 (function unknown)...")); 462 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = (dmpUpdates[pos]); 463 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 464 | 465 | DEBUG_PRINTLN(("Disabling all standby flags...")); 466 | I2Cdev::writeByte(0x68, MPU9150_RA_PWR_MGMT_2, 0x00); 467 | 468 | DEBUG_PRINTLN(("Setting accelerometer sensitivity to +/- 2g...")); 469 | I2Cdev::writeByte(0x68, MPU9150_RA_ACCEL_CONFIG, 0x00); 470 | 471 | DEBUG_PRINTLN(("Setting motion detection threshold to 2...")); 472 | setMotionDetectionThreshold(2); 473 | 474 | DEBUG_PRINTLN(("Setting zero-motion detection threshold to 156...")); 475 | setZeroMotionDetectionThreshold(156); 476 | 477 | DEBUG_PRINTLN(("Setting motion detection duration to 80...")); 478 | setMotionDetectionDuration(80); 479 | 480 | DEBUG_PRINTLN(("Setting zero-motion detection duration to 0...")); 481 | setZeroMotionDetectionDuration(0); 482 | 483 | DEBUG_PRINTLN(("Setting AK8975 to single measurement mode...")); 484 | //mag -> setMode(1); 485 | I2Cdev::writeByte(0x0E, 0x0A, 0x01); 486 | 487 | // setup AK8975 (0x0E) as Slave 0 in read mode 488 | DEBUG_PRINTLN(("Setting up AK8975 read slave 0...")); 489 | I2Cdev::writeByte(0x68, MPU9150_RA_I2C_SLV0_ADDR, 0x8E); 490 | I2Cdev::writeByte(0x68, MPU9150_RA_I2C_SLV0_REG, 0x01); 491 | I2Cdev::writeByte(0x68, MPU9150_RA_I2C_SLV0_CTRL, 0xDA); 492 | 493 | // setup AK8975 (0x0E) as Slave 2 in write mode 494 | DEBUG_PRINTLN(("Setting up AK8975 write slave 2...")); 495 | I2Cdev::writeByte(0x68, MPU9150_RA_I2C_SLV2_ADDR, 0x0E); 496 | I2Cdev::writeByte(0x68, MPU9150_RA_I2C_SLV2_REG, 0x0A); 497 | I2Cdev::writeByte(0x68, MPU9150_RA_I2C_SLV2_CTRL, 0x81); 498 | I2Cdev::writeByte(0x68, MPU9150_RA_I2C_SLV2_DO, 0x01); 499 | 500 | // setup I2C timing/delay control 501 | DEBUG_PRINTLN(("Setting up slave access delay...")); 502 | I2Cdev::writeByte(0x68, MPU9150_RA_I2C_SLV4_CTRL, 0x18); 503 | I2Cdev::writeByte(0x68, MPU9150_RA_I2C_MST_DELAY_CTRL, 0x05); 504 | 505 | // enable interrupts 506 | DEBUG_PRINTLN(("Enabling default interrupt behavior/no bypass...")); 507 | I2Cdev::writeByte(0x68, MPU9150_RA_INT_PIN_CFG, 0x00); 508 | 509 | // enable I2C master mode and reset DMP/FIFO 510 | DEBUG_PRINTLN(("Enabling I2C master mode...")); 511 | I2Cdev::writeByte(0x68, MPU9150_RA_USER_CTRL, 0x20); 512 | DEBUG_PRINTLN(("Resetting FIFO...")); 513 | I2Cdev::writeByte(0x68, MPU9150_RA_USER_CTRL, 0x24); 514 | DEBUG_PRINTLN(("Rewriting I2C master mode enabled because...I don't know")); 515 | I2Cdev::writeByte(0x68, MPU9150_RA_USER_CTRL, 0x20); 516 | DEBUG_PRINTLN(("Enabling and resetting DMP/FIFO...")); 517 | I2Cdev::writeByte(0x68, MPU9150_RA_USER_CTRL, 0xE8); 518 | 519 | DEBUG_PRINTLN(("Writing final memory update 5/19 (function unknown)...")); 520 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = (dmpUpdates[pos]); 521 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 522 | DEBUG_PRINTLN(("Writing final memory update 6/19 (function unknown)...")); 523 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = (dmpUpdates[pos]); 524 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 525 | DEBUG_PRINTLN(("Writing final memory update 7/19 (function unknown)...")); 526 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = (dmpUpdates[pos]); 527 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 528 | DEBUG_PRINTLN(("Writing final memory update 8/19 (function unknown)...")); 529 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = (dmpUpdates[pos]); 530 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 531 | DEBUG_PRINTLN(("Writing final memory update 9/19 (function unknown)...")); 532 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = (dmpUpdates[pos]); 533 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 534 | DEBUG_PRINTLN(("Writing final memory update 10/19 (function unknown)...")); 535 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = (dmpUpdates[pos]); 536 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 537 | DEBUG_PRINTLN(("Writing final memory update 11/19 (function unknown)...")); 538 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = (dmpUpdates[pos]); 539 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 540 | 541 | DEBUG_PRINTLN(("Reading final memory update 12/19 (function unknown)...")); 542 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = (dmpUpdates[pos]); 543 | readMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 544 | #ifdef DEBUG 545 | DEBUG_PRINT(("Read bytes: ")); 546 | for (j = 0; j < 4; j++) { 547 | DEBUG_PRINTF(dmpUpdate[3 + j], HEX); 548 | DEBUG_PRINT(" "); 549 | } 550 | DEBUG_PRINTLN(""); 551 | #endif 552 | 553 | DEBUG_PRINTLN(("Writing final memory update 13/19 (function unknown)...")); 554 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = (dmpUpdates[pos]); 555 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 556 | DEBUG_PRINTLN(("Writing final memory update 14/19 (function unknown)...")); 557 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = (dmpUpdates[pos]); 558 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 559 | DEBUG_PRINTLN(("Writing final memory update 15/19 (function unknown)...")); 560 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = (dmpUpdates[pos]); 561 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 562 | DEBUG_PRINTLN(("Writing final memory update 16/19 (function unknown)...")); 563 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = (dmpUpdates[pos]); 564 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 565 | DEBUG_PRINTLN(("Writing final memory update 17/19 (function unknown)...")); 566 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = (dmpUpdates[pos]); 567 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 568 | 569 | DEBUG_PRINTLN(("Waiting for FIRO count >= 46...")); 570 | while ((fifoCount = getFIFOCount()) < 46); 571 | DEBUG_PRINTLN(("Reading FIFO...")); 572 | getFIFOBytes(fifoBuffer, min(fifoCount, 128)); // safeguard only 128 bytes 573 | DEBUG_PRINTLN(("Reading interrupt status...")); 574 | getIntStatus(); 575 | 576 | DEBUG_PRINTLN(("Writing final memory update 18/19 (function unknown)...")); 577 | // Serial.println(pos); 578 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = (dmpUpdates[pos]); 579 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 580 | 581 | DEBUG_PRINTLN(("Waiting for FIRO count >= 48...")); 582 | while ((fifoCount = getFIFOCount()) < 48); 583 | DEBUG_PRINTLN(("Reading FIFO...")); 584 | getFIFOBytes(fifoBuffer, min(fifoCount, 128)); // safeguard only 128 bytes 585 | DEBUG_PRINTLN(("Reading interrupt status...")); 586 | getIntStatus(); 587 | DEBUG_PRINTLN(("Waiting for FIRO count >= 48...")); 588 | while ((fifoCount = getFIFOCount()) < 48); 589 | DEBUG_PRINTLN(("Reading FIFO...")); 590 | getFIFOBytes(fifoBuffer, min(fifoCount, 128)); // safeguard only 128 bytes 591 | DEBUG_PRINTLN(("Reading interrupt status...")); 592 | getIntStatus(); 593 | DEBUG_PRINTLN(("Writing final memory update 19/19 (function unknown)...")); 594 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = (dmpUpdates[pos]); 595 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 596 | 597 | DEBUG_PRINTLN(("Disabling DMP (you turn it on later)...")); 598 | setDMPEnabled(false); 599 | 600 | DEBUG_PRINTLN(("Setting up internal 48-byte (default) DMP packet buffer...")); 601 | dmpPacketSize = 48; 602 | /*if ((dmpPacketBuffer = (uint8_t *)malloc(42)) == 0) { 603 | return 3; // TODO: proper error code for no memory 604 | }*/ 605 | 606 | DEBUG_PRINTLN(("Resetting FIFO and clearing INT status one last time...")); 607 | resetFIFO(); 608 | getIntStatus(); 609 | } else { 610 | DEBUG_PRINTLN(("ERROR! DMP configuration verification failed.")); 611 | return 2; // configuration block loading failed 612 | } 613 | } else { 614 | DEBUG_PRINTLN(("ERROR! DMP code verification failed.")); 615 | return 1; // main binary block loading failed 616 | } 617 | return 0; // success 618 | } 619 | 620 | bool MPU9150::dmpPacketAvailable() { 621 | return getFIFOCount() >= dmpGetFIFOPacketSize(); 622 | } 623 | 624 | // uint8_t MPU9150::dmpSetFIFORate(uint8_t fifoRate); 625 | // uint8_t MPU9150::dmpGetFIFORate(); 626 | // uint8_t MPU9150::dmpGetSampleStepSizeMS(); 627 | // uint8_t MPU9150::dmpGetSampleFrequency(); 628 | // int32_t MPU9150::dmpDecodeTemperature(int8_t tempReg); 629 | 630 | //uint8_t MPU9150::dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority); 631 | //uint8_t MPU9150::dmpUnregisterFIFORateProcess(inv_obj_func func); 632 | //uint8_t MPU9150::dmpRunFIFORateProcesses(); 633 | 634 | // uint8_t MPU9150::dmpSendQuaternion(uint_fast16_t accuracy); 635 | // uint8_t MPU9150::dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy); 636 | // uint8_t MPU9150::dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy); 637 | // uint8_t MPU9150::dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy); 638 | // uint8_t MPU9150::dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy); 639 | // uint8_t MPU9150::dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy); 640 | // uint8_t MPU9150::dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy); 641 | // uint8_t MPU9150::dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy); 642 | // uint8_t MPU9150::dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy); 643 | // uint8_t MPU9150::dmpSendPacketNumber(uint_fast16_t accuracy); 644 | // uint8_t MPU9150::dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy); 645 | // uint8_t MPU9150::dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy); 646 | 647 | uint8_t MPU9150::dmpGetAccel(int32_t *data, const uint8_t* packet) { 648 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) 649 | if (packet == 0) packet = dmpPacketBuffer; 650 | data[0] = ((packet[34] << 24) + (packet[35] << 16) + (packet[36] << 8) + packet[37]); 651 | data[1] = ((packet[38] << 24) + (packet[39] << 16) + (packet[40] << 8) + packet[41]); 652 | data[2] = ((packet[42] << 24) + (packet[43] << 16) + (packet[44] << 8) + packet[45]); 653 | return 0; 654 | } 655 | uint8_t MPU9150::dmpGetAccel(int16_t *data, const uint8_t* packet) { 656 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) 657 | if (packet == 0) packet = dmpPacketBuffer; 658 | data[0] = (packet[34] << 8) + packet[35]; 659 | data[1] = (packet[38] << 8) + packet[39]; 660 | data[2] = (packet[42] << 8) + packet[43]; 661 | return 0; 662 | } 663 | uint8_t MPU9150::dmpGetAccel(VectorInt16 *v, const uint8_t* packet) { 664 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) 665 | if (packet == 0) packet = dmpPacketBuffer; 666 | v -> x = (packet[34] << 8) + packet[35]; 667 | v -> y = (packet[38] << 8) + packet[39]; 668 | v -> z = (packet[42] << 8) + packet[43]; 669 | return 0; 670 | } 671 | uint8_t MPU9150::dmpGetQuaternion(int32_t *data, const uint8_t* packet) { 672 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) 673 | if (packet == 0) packet = dmpPacketBuffer; 674 | data[0] = ((packet[0] << 24) + (packet[1] << 16) + (packet[2] << 8) + packet[3]); 675 | data[1] = ((packet[4] << 24) + (packet[5] << 16) + (packet[6] << 8) + packet[7]); 676 | data[2] = ((packet[8] << 24) + (packet[9] << 16) + (packet[10] << 8) + packet[11]); 677 | data[3] = ((packet[12] << 24) + (packet[13] << 16) + (packet[14] << 8) + packet[15]); 678 | return 0; 679 | } 680 | uint8_t MPU9150::dmpGetQuaternion(int16_t *data, const uint8_t* packet) { 681 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) 682 | if (packet == 0) packet = dmpPacketBuffer; 683 | data[0] = ((packet[0] << 8) + packet[1]); 684 | data[1] = ((packet[4] << 8) + packet[5]); 685 | data[2] = ((packet[8] << 8) + packet[9]); 686 | data[3] = ((packet[12] << 8) + packet[13]); 687 | return 0; 688 | } 689 | uint8_t MPU9150::dmpGetQuaternion(Quaternion *q, const uint8_t* packet) { 690 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) 691 | int16_t qI[4]; 692 | uint8_t status = dmpGetQuaternion(qI, packet); 693 | if (status == 0) { 694 | q -> w = (float)qI[0] / 16384.0f; 695 | q -> x = (float)qI[1] / 16384.0f; 696 | q -> y = (float)qI[2] / 16384.0f; 697 | q -> z = (float)qI[3] / 16384.0f; 698 | return 0; 699 | } 700 | return status; // int16 return value, indicates error if this line is reached 701 | } 702 | // uint8_t MPU9150::dmpGet6AxisQuaternion(long *data, const uint8_t* packet); 703 | // uint8_t MPU9150::dmpGetRelativeQuaternion(long *data, const uint8_t* packet); 704 | uint8_t MPU9150::dmpGetGyro(int32_t *data, const uint8_t* packet) { 705 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) 706 | if (packet == 0) packet = dmpPacketBuffer; 707 | data[0] = ((packet[16] << 24) + (packet[17] << 16) + (packet[18] << 8) + packet[19]); 708 | data[1] = ((packet[20] << 24) + (packet[21] << 16) + (packet[22] << 8) + packet[23]); 709 | data[2] = ((packet[24] << 24) + (packet[25] << 16) + (packet[26] << 8) + packet[27]); 710 | return 0; 711 | } 712 | uint8_t MPU9150::dmpGetGyro(int16_t *data, const uint8_t* packet) { 713 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) 714 | if (packet == 0) packet = dmpPacketBuffer; 715 | data[0] = (packet[16] << 8) + packet[17]; 716 | data[1] = (packet[20] << 8) + packet[21]; 717 | data[2] = (packet[24] << 8) + packet[25]; 718 | return 0; 719 | } 720 | uint8_t MPU9150::dmpGetMag(int16_t *data, const uint8_t* packet) { 721 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) 722 | if (packet == 0) packet = dmpPacketBuffer; 723 | data[0] = (packet[28] << 8) + packet[29]; 724 | data[1] = (packet[30] << 8) + packet[31]; 725 | data[2] = (packet[32] << 8) + packet[33]; 726 | return 0; 727 | } 728 | // uint8_t MPU9150::dmpSetLinearAccelFilterCoefficient(float coef); 729 | // uint8_t MPU9150::dmpGetLinearAccel(long *data, const uint8_t* packet); 730 | uint8_t MPU9150::dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity) { 731 | // get rid of the gravity component (+1g = +4096 in standard DMP FIFO packet) 732 | v -> x = vRaw -> x - gravity -> x*4096; 733 | v -> y = vRaw -> y - gravity -> y*4096; 734 | v -> z = vRaw -> z - gravity -> z*4096; 735 | return 0; 736 | } 737 | // uint8_t MPU9150::dmpGetLinearAccelInWorld(long *data, const uint8_t* packet); 738 | uint8_t MPU9150::dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q) { 739 | // rotate measured 3D acceleration vector into original state 740 | // frame of reference based on orientation quaternion 741 | memcpy(v, vReal, sizeof(VectorInt16)); 742 | v -> rotate(q); 743 | return 0; 744 | } 745 | // uint8_t MPU9150::dmpGetGyroAndAccelSensor(long *data, const uint8_t* packet); 746 | // uint8_t MPU9150::dmpGetGyroSensor(long *data, const uint8_t* packet); 747 | // uint8_t MPU9150::dmpGetControlData(long *data, const uint8_t* packet); 748 | // uint8_t MPU9150::dmpGetTemperature(long *data, const uint8_t* packet); 749 | // uint8_t MPU9150::dmpGetGravity(long *data, const uint8_t* packet); 750 | uint8_t MPU9150::dmpGetGravity(VectorFloat *v, Quaternion *q) { 751 | v -> x = 2 * (q -> x*q -> z - q -> w*q -> y); 752 | v -> y = 2 * (q -> w*q -> x + q -> y*q -> z); 753 | v -> z = q -> w*q -> w - q -> x*q -> x - q -> y*q -> y + q -> z*q -> z; 754 | return 0; 755 | } 756 | // uint8_t MPU9150::dmpGetUnquantizedAccel(long *data, const uint8_t* packet); 757 | // uint8_t MPU9150::dmpGetQuantizedAccel(long *data, const uint8_t* packet); 758 | // uint8_t MPU9150::dmpGetExternalSensorData(long *data, int size, const uint8_t* packet); 759 | // uint8_t MPU9150::dmpGetEIS(long *data, const uint8_t* packet); 760 | 761 | uint8_t MPU9150::dmpGetEuler(float *data, Quaternion *q) { 762 | 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 763 | data[1] = -asin(2*q -> x*q -> z + 2*q -> w*q -> y); // theta 764 | 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 765 | return 0; 766 | } 767 | uint8_t MPU9150::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) { 768 | // yaw: (about Z axis) 769 | data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1); 770 | // pitch: (nose up/down, about Y axis) 771 | data[1] = atan(gravity -> x / sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z)); 772 | // roll: (tilt left/right, about X axis) 773 | data[2] = atan(gravity -> y / sqrt(gravity -> x*gravity -> x + gravity -> z*gravity -> z)); 774 | return 0; 775 | } 776 | 777 | // uint8_t MPU9150::dmpGetAccelFloat(float *data, const uint8_t* packet); 778 | // uint8_t MPU9150::dmpGetQuaternionFloat(float *data, const uint8_t* packet); 779 | 780 | uint8_t MPU9150::dmpProcessFIFOPacket(const unsigned char *dmpData) { 781 | /*for (uint8_t k = 0; k < dmpPacketSize; k++) { 782 | if (dmpData[k] < 0x10) Serial.print("0"); 783 | Serial.print(dmpData[k], HEX); 784 | Serial.print(" "); 785 | } 786 | Serial.print("\n");*/ 787 | //Serial.println((uint16_t)dmpPacketBuffer); 788 | return 0; 789 | } 790 | uint8_t MPU9150::dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed) { 791 | uint8_t status; 792 | uint8_t buf[dmpPacketSize]; 793 | for (uint8_t i = 0; i < numPackets; i++) { 794 | // read packet from FIFO 795 | getFIFOBytes(buf, dmpPacketSize); 796 | 797 | // process packet 798 | if ((status = dmpProcessFIFOPacket(buf)) > 0) return status; 799 | 800 | // increment external process count variable, if supplied 801 | if (processed != 0) *processed++; 802 | } 803 | return 0; 804 | } 805 | 806 | // uint8_t MPU9150::dmpSetFIFOProcessedCallback(void (*func) (void)); 807 | 808 | // uint8_t MPU9150::dmpInitFIFOParam(); 809 | // uint8_t MPU9150::dmpCloseFIFO(); 810 | // uint8_t MPU9150::dmpSetGyroDataSource(uint_fast8_t source); 811 | // uint8_t MPU9150::dmpDecodeQuantizedAccel(); 812 | // uint32_t MPU9150::dmpGetGyroSumOfSquare(); 813 | // uint32_t MPU9150::dmpGetAccelSumOfSquare(); 814 | // void MPU9150::dmpOverrideQuaternion(long *q); 815 | uint16_t MPU9150::dmpGetFIFOPacketSize() { 816 | return dmpPacketSize; 817 | } 818 | 819 | #endif /* _MPU9150_9AXIS_MOTIONAPPS41_H_ */ 820 | -------------------------------------------------------------------------------- /sensors/MPU/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 | #include "math.h" 36 | #include "inttypes.h" 37 | class Quaternion { 38 | public: 39 | float w; 40 | float x; 41 | float y; 42 | float z; 43 | 44 | Quaternion() { 45 | w = 1.0f; 46 | x = 0.0f; 47 | y = 0.0f; 48 | z = 0.0f; 49 | } 50 | 51 | Quaternion(float nw, float nx, float ny, float nz) { 52 | w = nw; 53 | x = nx; 54 | y = ny; 55 | z = nz; 56 | } 57 | 58 | Quaternion getProduct(Quaternion q) { 59 | // Quaternion multiplication is defined by: 60 | // (Q1 * Q2).w = (w1w2 - x1x2 - y1y2 - z1z2) 61 | // (Q1 * Q2).x = (w1x2 + x1w2 + y1z2 - z1y2) 62 | // (Q1 * Q2).y = (w1y2 - x1z2 + y1w2 + z1x2) 63 | // (Q1 * Q2).z = (w1z2 + x1y2 - y1x2 + z1w2 64 | return Quaternion( 65 | w*q.w - x*q.x - y*q.y - z*q.z, // new w 66 | w*q.x + x*q.w + y*q.z - z*q.y, // new x 67 | w*q.y - x*q.z + y*q.w + z*q.x, // new y 68 | w*q.z + x*q.y - y*q.x + z*q.w); // new z 69 | } 70 | 71 | Quaternion getConjugate() { 72 | return Quaternion(w, -x, -y, -z); 73 | } 74 | 75 | float getMagnitude() { 76 | return sqrt(w*w + x*x + y*y + z*z); 77 | } 78 | 79 | void normalize() { 80 | float m = getMagnitude(); 81 | w /= m; 82 | x /= m; 83 | y /= m; 84 | z /= m; 85 | } 86 | 87 | Quaternion getNormalized() { 88 | Quaternion r(w, x, y, z); 89 | r.normalize(); 90 | return r; 91 | } 92 | }; 93 | 94 | class VectorInt16 { 95 | public: 96 | int16_t x; 97 | int16_t y; 98 | int16_t z; 99 | 100 | VectorInt16() { 101 | x = 0; 102 | y = 0; 103 | z = 0; 104 | } 105 | 106 | VectorInt16(int16_t nx, int16_t ny, int16_t nz) { 107 | x = nx; 108 | y = ny; 109 | z = nz; 110 | } 111 | 112 | float getMagnitude() { 113 | return sqrt(x*x + y*y + z*z); 114 | } 115 | 116 | void normalize() { 117 | float m = getMagnitude(); 118 | x /= m; 119 | y /= m; 120 | z /= m; 121 | } 122 | 123 | VectorInt16 getNormalized() { 124 | VectorInt16 r(x, y, z); 125 | r.normalize(); 126 | return r; 127 | } 128 | 129 | void rotate(Quaternion *q) { 130 | // http://www.cprogramming.com/tutorial/3d/quaternions.html 131 | // http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/transforms/index.htm 132 | // http://content.gpwiki.org/index.php/OpenGL:Tutorials:Using_Quaternions_to_represent_rotation 133 | // ^ 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 134 | 135 | // P_out = q * P_in * conj(q) 136 | // - P_out is the output vector 137 | // - q is the orientation quaternion 138 | // - P_in is the input vector (a*aReal) 139 | // - conj(q) is the conjugate of the orientation quaternion (q=[w,x,y,z], q*=[w,-x,-y,-z]) 140 | Quaternion p(0, x, y, z); 141 | 142 | // quaternion multiplication: q * p, stored back in p 143 | p = q -> getProduct(p); 144 | 145 | // quaternion multiplication: p * conj(q), stored back in p 146 | p = p.getProduct(q -> getConjugate()); 147 | 148 | // p quaternion is now [0, x', y', z'] 149 | x = p.x; 150 | y = p.y; 151 | z = p.z; 152 | } 153 | 154 | VectorInt16 getRotated(Quaternion *q) { 155 | VectorInt16 r(x, y, z); 156 | r.rotate(q); 157 | return r; 158 | } 159 | }; 160 | 161 | class VectorFloat { 162 | public: 163 | float x; 164 | float y; 165 | float z; 166 | 167 | VectorFloat() { 168 | x = 0; 169 | y = 0; 170 | z = 0; 171 | } 172 | 173 | VectorFloat(float nx, float ny, float nz) { 174 | x = nx; 175 | y = ny; 176 | z = nz; 177 | } 178 | 179 | float getMagnitude() { 180 | return sqrt(x*x + y*y + z*z); 181 | } 182 | 183 | void normalize() { 184 | float m = getMagnitude(); 185 | x /= m; 186 | y /= m; 187 | z /= m; 188 | } 189 | 190 | VectorFloat getNormalized() { 191 | VectorFloat r(x, y, z); 192 | r.normalize(); 193 | return r; 194 | } 195 | 196 | void rotate(Quaternion *q) { 197 | Quaternion p(0, x, y, z); 198 | 199 | // quaternion multiplication: q * p, stored back in p 200 | p = q -> getProduct(p); 201 | 202 | // quaternion multiplication: p * conj(q), stored back in p 203 | p = p.getProduct(q -> getConjugate()); 204 | 205 | // p quaternion is now [0, x', y', z'] 206 | x = p.x; 207 | y = p.y; 208 | z = p.z; 209 | } 210 | 211 | VectorFloat getRotated(Quaternion *q) { 212 | VectorFloat r(x, y, z); 213 | r.rotate(q); 214 | return r; 215 | } 216 | }; 217 | 218 | #endif /* _HELPER_3DMATH_H_ */ -------------------------------------------------------------------------------- /sensors/MPUSensor.h: -------------------------------------------------------------------------------- 1 | /* 2 | MPUSensor.h - Reliable Library that controlls both MPU9150 and MPU6050 3 | 4 | For instructions, go to https://github.com/ivanseidel/Arduino-Sensors 5 | 6 | Created by Ivan Seidel Gomes, June, 2013. 7 | Released into the public domain. 8 | */ 9 | 10 | #ifndef IMU_h 11 | #define IMU_h 12 | 13 | #include 14 | #include 15 | 16 | #include 17 | #include "I2Cdev.h" 18 | #include "sensors/MPU/dmpMPU9150.h" 19 | #include 20 | #include 21 | 22 | class MPUSensor: public Thread, public AngleInterface{ 23 | 24 | protected: 25 | uint8_t _fifoBuffer[64]; 26 | uint8_t _packetSize; 27 | uint8_t _fifoCount; 28 | 29 | // uint8_t teapotPacket[14]; 30 | 31 | public: 32 | 33 | //int16_t ax, ay, az, gx, gy, gz, mx, my, mz; 34 | 35 | // Flag that indicates if there is new data available 36 | bool hasNewData; 37 | 38 | /* 39 | Those flags can be manipulated. 40 | When enabled, the corresponding measurement will 41 | be always refreshed whenever a new sample is read 42 | from the MPU. 43 | */ 44 | // Raw Gravity 45 | bool readGravity; 46 | // Raw data 47 | bool readAccel; 48 | // Linear Acelleration (Gravity Free) 49 | bool readAccelReal; 50 | // World-frame Acelleration (Gravity Free) 51 | bool readAccelWorld; 52 | // Yaw, Pitch and Roll readings 53 | bool readYPR; 54 | 55 | // Return types (corresponds to the flags above) 56 | Quaternion quaternion; 57 | VectorInt16 accel; 58 | VectorInt16 accelReal; 59 | VectorInt16 accelWorld; 60 | VectorFloat gravity; 61 | float ypr[3]; // In RADIANS 62 | 63 | // The MPU Device 64 | MPU9150 device; 65 | 66 | MPUSensor(TwoWire TWI){ 67 | // Initialize local flags 68 | setDefaults(); 69 | // Public flag, to indicate if there is new data proccessed 70 | hasNewData = false; 71 | // Set current TWI on I2Cdev 72 | I2Cdev::Wire = TWI; 73 | TWI.begin(); 74 | // Instantiate device 75 | device = MPU9150(); 76 | 77 | //ax = ay = az = 0; 78 | //gx = gy = gz = 0; 79 | //mx = my = mz = 0; 80 | 81 | // teapotPacket = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' }; 82 | }; 83 | 84 | /* 85 | This will initialize the DMP and make the MPU ready to begin. 86 | 87 | the return of this function indicates if it sucessfully 88 | loaded the DMP and the device is ready to start sampling. 89 | 90 | true: OK | false: FAIL 91 | */ 92 | virtual bool initialize(){ 93 | 94 | // Disable DMP 95 | device.resetDMP(); 96 | // delay(20); 97 | device.setDMPEnabled(false); 98 | // delay(20); 99 | // Reset IMU 100 | device.reset(); 101 | // delay(20); 102 | // Initialize 103 | device.initialize(); 104 | // delay(20); 105 | // Test connection 106 | // if(!device.testConnection()) Serial.println("FAIL CONNECT"); 107 | // if(!device.testConnection()) return false; 108 | // Configure DMP (0 = Success) 109 | if(device.dmpInitialize() != 0) return false; 110 | // Read the returning packet size 111 | _packetSize = device.dmpGetFIFOPacketSize(); 112 | Serial.println(_packetSize); 113 | 114 | return true; 115 | } 116 | 117 | /* 118 | Whenever you want to start samping, call this function 119 | */ 120 | virtual void start(){ 121 | // Enable DMP 122 | device.setDMPEnabled(false); 123 | delay(100); 124 | device.setDMPEnabled(true); 125 | } 126 | 127 | /* 128 | When you are done, stop the DMP 129 | */ 130 | virtual void stop(){ 131 | // Disable DMP 132 | device.setDMPEnabled(false); 133 | } 134 | 135 | // Return flags to false (no reading will be done) 136 | virtual void setDefaults(){ 137 | readGravity = false; 138 | readAccel = false; 139 | readAccelReal = false; 140 | readAccelWorld = false; 141 | readYPR = true; 142 | } 143 | 144 | /*void update9() { 145 | device.getMotion9(&ax, &ay, &az, 146 | &gx, &gy, &gz, 147 | &mx, &my, &mz); 148 | }*/ 149 | 150 | /* 151 | This Method is responsable for syncronizing the MPU. 152 | 153 | You should call this CONSTANTLY. Consider inserting it 154 | inside a Timer, and calling it from there. 155 | 156 | Also, This class extends Thread, witch means that it can 157 | be registered inside a ThreadController. Configure the 158 | sampling time with setInterval(); 159 | 160 | Whenever a new sample is fresh, hasNewData will go true. 161 | */ 162 | virtual void run(){ 163 | if (_fifoCount = device.getFIFOCount() >= _packetSize){ 164 | // Serial.println("\tRead"); 165 | 166 | // read a packet from FIFO 167 | device.getFIFOBytes(_fifoBuffer, _packetSize); 168 | device.resetFIFO(); 169 | 170 | // Base data to be proccessed 171 | device.dmpGetQuaternion(&quaternion, _fifoBuffer); 172 | 173 | // Process Accel 174 | if(readAccel || readAccelWorld || readAccelReal) 175 | device.dmpGetAccel(&accel, _fifoBuffer); 176 | 177 | // Process Gravity 178 | if(readGravity || readYPR || readAccelReal || readAccelWorld) 179 | device.dmpGetGravity(&gravity, &quaternion); 180 | 181 | // Process YPR (IN RADIANS) 182 | if(readYPR) 183 | device.dmpGetYawPitchRoll(ypr, &quaternion, &gravity); 184 | 185 | if(readAccelReal) 186 | device.dmpGetLinearAccel(&accelReal, &accel, &gravity); 187 | 188 | if(readAccelWorld) 189 | device.dmpGetLinearAccelInWorld(&accelWorld, &accelReal, &quaternion); 190 | 191 | 192 | // Set flag that new data has come 193 | hasNewData = true; 194 | 195 | // This is the default DEMO from Invensense 196 | /*teapotPacket[2] = _fifoBuffer[0]; 197 | teapotPacket[3] = _fifoBuffer[1]; 198 | teapotPacket[4] = _fifoBuffer[4]; 199 | teapotPacket[5] = _fifoBuffer[5]; 200 | teapotPacket[6] = _fifoBuffer[8]; 201 | teapotPacket[7] = _fifoBuffer[9]; 202 | teapotPacket[8] = _fifoBuffer[12]; 203 | teapotPacket[9] = _fifoBuffer[13]; 204 | Serial.write(teapotPacket, 14); 205 | teapotPacket[11]++;*/ 206 | runned(); 207 | 208 | } 209 | }; 210 | 211 | /* 212 | Returns the Yaw value of the MPU 213 | */ 214 | virtual float getAngle(){ 215 | // Default angle returned is Yaw 216 | // TODO: Return selected angle 217 | return ypr[0]; 218 | } 219 | 220 | /* 221 | "Tries" to read and return the fresh value of the MPU 222 | */ 223 | virtual float readValue(){ 224 | // Run self Thread and tries to update 225 | run(); 226 | 227 | return getAngle(); 228 | } 229 | 230 | }; 231 | 232 | #endif -------------------------------------------------------------------------------- /sensors/PingUltrasonic.h: -------------------------------------------------------------------------------- 1 | /* 2 | Ultrasonic.h - Library for Ping and HR-SC04 Ultrasonic Ranging Module. 3 | 4 | In order to use PING sensors (One wire for data only), use: 5 | PingUltrasonic myUltrasonic(dataPin); 6 | 7 | In order to use with two wire (Trigger and Echo) sensors, use: 8 | PingUltrasonic myUltrasonic(triggerPin, echoPin); 9 | 10 | 11 | Working range: 3 - 500 12 | 13 | For instructions, go to https://github.com/ivanseidel/Arduino-Sensors 14 | 15 | Modified by Ivan Seidel Gomes, June, 2013. 16 | Released into the public domain. 17 | */ 18 | 19 | #ifndef Ultrasonic_h 20 | #define Ultrasonic_h 21 | 22 | #include 23 | 24 | #include 25 | #include 26 | 27 | class PingUltrasonic: public Thread, public DistanceInterface 28 | { 29 | private: 30 | int trigPin; 31 | int echoPin; 32 | 33 | long duration; 34 | float distance; 35 | 36 | void sendCommand(){ 37 | // Set pin as output 38 | if(trigPin == echoPin) 39 | pinMode(trigPin, OUTPUT); 40 | 41 | // Make shure it's LOW (Erase state) 42 | digitalWrite(trigPin, LOW); 43 | delayMicroseconds(2); 44 | 45 | // Send pulse 46 | digitalWrite(trigPin, HIGH); 47 | delayMicroseconds(10); 48 | digitalWrite(trigPin, LOW); 49 | 50 | // Set pin as input 51 | if(trigPin == echoPin) 52 | pinMode(echoPin, INPUT); 53 | } 54 | 55 | bool waitResponse(){ 56 | long startTime = micros(); 57 | 58 | // Waits it to get HIGH 59 | while(digitalRead(echoPin) == LOW){ 60 | if(micros() - startTime > 1000){ 61 | return false; 62 | break; 63 | } 64 | } 65 | return true; 66 | } 67 | 68 | public: 69 | PingUltrasonic(int _trigPin, int _echoPin = -1): Thread() { 70 | trigPin = _trigPin; 71 | 72 | /* 73 | Ping sensors uses only one pin to trigger and echo 74 | */ 75 | if(echoPin < 0) 76 | echoPin = trigPin; 77 | else 78 | echoPin = _echoPin; 79 | 80 | pinMode(trigPin, OUTPUT); 81 | pinMode(echoPin, INPUT); 82 | 83 | minDistance = 3; 84 | maxDistance = 500; 85 | 86 | setInterval(50); 87 | } 88 | 89 | virtual float getDistance(){ 90 | return distance; 91 | } 92 | 93 | virtual float readDistance(){ 94 | // Send pulse to trigger reading action 95 | sendCommand(); 96 | 97 | /* 98 | After reading, if this flag is true, indicates either: 99 | * Sensor is not connected (echo is not responding) 100 | * Timeout on reading pulse 101 | */ 102 | bool error = false; 103 | 104 | // Start time to compare later 105 | long startTime = micros(); 106 | 107 | // Waits for the pulse, and check if returned true 108 | if(waitResponse()){ 109 | 110 | // Reads HIGH pulse time 111 | while(digitalRead(echoPin) == HIGH){ 112 | if(micros() - startTime > 50000){ 113 | error = true; 114 | // Serial.println("FAIL2!"); 115 | // Serial.print("FAIL2!\t"); 116 | break; 117 | } 118 | } 119 | 120 | }else{ 121 | error = true; 122 | } 123 | 124 | // Calculates duration 125 | duration = micros() - startTime; 126 | 127 | // If error, then doesen't update value 128 | if(!error) 129 | distance = duration / 29.0 / 2; 130 | else 131 | distance = maxDistance; 132 | 133 | return getDistance(); 134 | } 135 | 136 | /* 137 | Check if the sensor is connected 138 | */ 139 | virtual bool isConnected(){ 140 | sendCommand(); 141 | return waitResponse(); 142 | } 143 | 144 | /* 145 | Thread implementation 146 | */ 147 | virtual void run(){ 148 | readDistance(); 149 | 150 | runned(); 151 | } 152 | 153 | }; 154 | 155 | extern PingUltrasonic Ultrasonic; 156 | 157 | #endif 158 | -------------------------------------------------------------------------------- /sensors/SharpInterface.h: -------------------------------------------------------------------------------- 1 | /* 2 | SharpInterface.h - Sharp Infrared Distance Sensor Interface 3 | 4 | This class provides basic mathods for all Sharp Sensors. 5 | 6 | For instructions, go to https://github.com/ivanseidel/Arduino-Sensors 7 | 8 | Created by Ivan Seidel Gomes, 9 | André Seidel Oliveira, June, 2013. 10 | Released into the public domain. 11 | */ 12 | 13 | #ifndef SharpInterface_h 14 | #define SharpInterface_h 15 | 16 | #include 17 | #include 18 | #include 19 | 20 | class SharpInterface: public Thread, public DistanceInterface 21 | { 22 | protected: 23 | // Cached Distance of the sensor 24 | float distance; 25 | 26 | // AnalogInput of the sensor 27 | AnalogIn analogPin; 28 | 29 | /* 30 | This method is responsable for converting the 31 | corresponding ADC value into CM value and returning 32 | */ 33 | virtual float convertDistance(float value){return value;} 34 | 35 | public: 36 | /* 37 | Initialize 38 | */ 39 | SharpInterface(int _pin){ 40 | analogPin = AnalogIn(_pin); 41 | 42 | distance = 0; 43 | 44 | Thread::Thread(); 45 | setInterval(20); 46 | } 47 | 48 | /* 49 | Returns the CACHED distance 50 | */ 51 | virtual float getDistance(){ 52 | return distance; 53 | } 54 | 55 | /* 56 | Reads, CACHE and returns the cached value 57 | */ 58 | virtual float readDistance(){ 59 | distance = convertDistance(analogPin.read()); 60 | 61 | return getDistance(); 62 | } 63 | 64 | /* 65 | Thread callback 66 | */ 67 | virtual void run(){ 68 | readDistance(); 69 | 70 | runned(); 71 | } 72 | 73 | }; 74 | 75 | #endif -------------------------------------------------------------------------------- /sensors/SharpLong.h: -------------------------------------------------------------------------------- 1 | /* 2 | SharpLong.h - Sharp Infrared Distance Sensor GP2Y0A02YK0F Library 3 | 4 | Working range: 30 - 90 5 | 6 | For instructions, go to https://github.com/ivanseidel/Arduino-Sensors 7 | 8 | Created by Ivan Seidel Gomes, 9 | André Seidel Oliveira, June, 2013. 10 | Released into the public domain. 11 | */ 12 | 13 | #ifndef SharpLong_h 14 | #define SharpLong_h 15 | 16 | #include 17 | 18 | class SharpLong: public SharpInterface 19 | { 20 | protected: 21 | /* 22 | Convert the value 23 | */ 24 | virtual float convertDistance(float value){ 25 | // distance = 16356.00 / (value - 57.35); 26 | float converted = 16356.00 / (value - 57.35); 27 | 28 | // Filter lower and high values 29 | if(converted > 60) 30 | converted = 17985.61 / (value - 30.18); 31 | 32 | if(converted <= 0) 33 | converted = maxDistance; 34 | 35 | converted = max(minDistance, min(maxDistance,converted)); 36 | 37 | return converted; 38 | } 39 | 40 | public: 41 | /* 42 | Initialize 43 | */ 44 | SharpLong(int _pin): SharpInterface(_pin){ 45 | minDistance = 30; 46 | maxDistance = 90; 47 | } 48 | 49 | }; 50 | 51 | #endif -------------------------------------------------------------------------------- /sensors/SharpMedium.h: -------------------------------------------------------------------------------- 1 | /* 2 | SharpMedium.h - Sharp Infrared Distance Sensor GP2Y0A21YK Library 3 | 4 | Working range: 6 - 45 5 | 6 | For instructions, go to https://github.com/ivanseidel/Arduino-Sensors 7 | 8 | Created by Ivan Seidel Gomes, 9 | André Seidel Oliveira, June, 2013. 10 | Released into the public domain. 11 | */ 12 | 13 | #ifndef SharpMedium_h 14 | #define SharpMedium_h 15 | 16 | #include 17 | 18 | class SharpMedium: public SharpInterface 19 | { 20 | protected: 21 | /* 22 | Convert the value 23 | */ 24 | virtual float convertDistance(float value){ 25 | // distance = 9019.84/(value + 20.36); 26 | float converted = 6570.66/(value - 16.0); 27 | 28 | // Filter lower and high values 29 | converted = max(minDistance, min(maxDistance,converted)); 30 | 31 | return converted; 32 | } 33 | 34 | public: 35 | /* 36 | Initialize 37 | */ 38 | SharpMedium(int _pin): SharpInterface(_pin){ 39 | minDistance = 6; 40 | maxDistance = 45; 41 | } 42 | 43 | }; 44 | 45 | #endif -------------------------------------------------------------------------------- /sensors/SharpShort.h: -------------------------------------------------------------------------------- 1 | /* 2 | SharpShort.h - Sharp Infrared Distance Sensor GP2D120XJ00F Library 3 | 4 | Working range: 7 - 30 5 | 6 | For instructions, go to https://github.com/ivanseidel/Arduino-Sensors 7 | 8 | Created by Ivan Seidel Gomes, 9 | André Seidel Oliveira, June, 2013. 10 | Released into the public domain. 11 | */ 12 | 13 | #ifndef SharpShort_h 14 | #define SharpShort_h 15 | 16 | #include 17 | 18 | class SharpShort: public SharpInterface 19 | { 20 | protected: 21 | /* 22 | Convert the value 23 | */ 24 | virtual float convertDistance(float value){ 25 | // distance = 11024.98 / (value +100.24) - 4.0; 26 | float converted = 5489.72 / (value - 40.03); 27 | 28 | // Filter lower and high values 29 | converted = max(minDistance, min(maxDistance,converted)); 30 | 31 | return converted; 32 | } 33 | 34 | public: 35 | /* 36 | Initialize 37 | */ 38 | SharpShort(int _pin): SharpInterface(_pin){ 39 | minDistance = 7; 40 | maxDistance = 30; 41 | } 42 | 43 | }; 44 | 45 | #endif --------------------------------------------------------------------------------