├── LICENSE.md ├── README.md ├── examples ├── MPU9250_Basic │ └── MPU9250_Basic.ino ├── MPU9250_Basic_Interrupt │ └── MPU9250_Basic_Interrupt.ino ├── MPU9250_DMP_Gyro_Cal │ └── MPU9250_DMP_Gyro_Cal.ino ├── MPU9250_DMP_Orientation │ └── MPU9250_DMP_Orientation.ino ├── MPU9250_DMP_Pedometer │ └── MPU9250_DMP_Pedometer.ino ├── MPU9250_DMP_Quaternion │ └── MPU9250_DMP_Quaternion.ino ├── MPU9250_DMP_Tap │ └── MPU9250_DMP_Tap.ino └── MPU9250_FIFO_Basic │ └── MPU9250_FIFO_Basic.ino ├── keywords.txt ├── library.properties └── src ├── MPU9250_RegisterMap.h ├── SparkFunMPU9250-DMP.cpp ├── SparkFunMPU9250-DMP.h └── util ├── MPU9250_RegisterMap.h ├── arduino_mpu9250_clk.c ├── arduino_mpu9250_clk.h ├── arduino_mpu9250_i2c.cpp ├── arduino_mpu9250_i2c.h ├── arduino_mpu9250_log.cpp ├── arduino_mpu9250_log.h ├── dmpKey.h ├── dmpmap.h ├── inv_mpu.c ├── inv_mpu.h ├── inv_mpu_dmp_motion_driver.c └── inv_mpu_dmp_motion_driver.h /LICENSE.md: -------------------------------------------------------------------------------- 1 | SparkFun License Information 2 | ============================ 3 | 4 | **SparkFun code, firmware, and software is released under the [MIT License](http://opensource.org/licenses/MIT).** 5 | 6 | The MIT License (MIT) 7 | 8 | Copyright (c) 2016 SparkFun Electronics, Inc. 9 | 10 | Permission is hereby granted, free of charge, to any person obtaining a copy 11 | of this software and associated documentation files (the "Software"), to deal 12 | in the Software without restriction, including without limitation the rights 13 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 14 | copies of the Software, and to permit persons to whom the Software is 15 | furnished to do so, subject to the following conditions: 16 | 17 | The above copyright notice and this permission notice shall be included in all 18 | copies or substantial portions of the Software. 19 | 20 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 21 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 22 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 23 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 24 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 25 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 26 | SOFTWARE. 27 | 28 | 29 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | SparkFun MPU-9250 Digital Motion Processor (DMP) Arduino Library 2 | ======================================== 3 | 4 | ### I have forked this repository to edit and make changes for the ESP32. Will submit a PR when ready. This is a Work in Progress 5 | 6 | [![SparkFun MPU-9250](https://cdn.sparkfun.com//assets/parts/1/1/3/0/6/13762-00a.jpg)](https://www.sparkfun.com/products/13762) 7 | 8 | [*SparkFun MPU-9250 (SEN-13762)*](https://www.sparkfun.com/products/13762) 9 | 10 | Advanced Arduino library for the Invensense MPU-9250 inertial measurement unit (IMU), which enables the sensor's digital motion processing (DMP) features. Along with configuring and reading from the accelerometer, gyroscope, and magnetometer, this library also supports the chip's DMP features like: 11 | 12 | * Quaternion calculation 13 | * Pedometer 14 | * Gyroscope calibration 15 | * Tap detection 16 | * Orientation dtection 17 | 18 | For help getting started with this library, refer to the [Using the MPU-9250 DMP Arduino Library](https://learn.sparkfun.com/tutorials/9dof-razor-imu-m0-hookup-guide#using-the-mpu-9250-dmp-arduino-library) section of the 9DoF Razor IMU M0 Hookup Guide. 19 | 20 | **Note**: This library currently only supports and is tested on **SAMD processors**. It's a major part of the [SparkFun 9DoF Razor IMU M0](https://www.sparkfun.com/products/14001) firmware. 21 | 22 | If you're looking for an AVR-compatible (Arduino Uno, SparkFun RedBoard, etc.) library, check out the [SparkFun MPU-9250 Breakout Library](https://github.com/sparkfun/SparkFun_MPU-9250_Breakout_Arduino_Library), which provides access to all standard MPU-9250 sensor-readings. 23 | 24 | Repository Contents 25 | ------------------- 26 | 27 | * **/examples** - Example sketches for the library (.ino). Run these from the Arduino IDE. 28 | * **/src** - Source files for the library (.cpp, .h). 29 | * **/src/util** - Source and headers for the MPU-9250 driver and dmp configuration. These are available and adapted from [Invensene's downloads page](https://www.invensense.com/developers/software-downloads/#sla_content_45). 30 | * **keywords.txt** - Keywords from this library that will be highlighted in the Arduino IDE. 31 | * **library.properties** - General library properties for the Arduino package manager. 32 | 33 | Documentation 34 | -------------- 35 | 36 | * **[Installing an Arduino Library Guide](https://learn.sparkfun.com/tutorials/installing-an-arduino-library)** - Basic information on how to install an Arduino library. 37 | * **[SparkFun 9DoF Razor IMU M0 Repository](https://github.com/sparkfun/9DOF_Razor_IMU)** - Main repositor (including hardware files) for the MPU-9250-based SparkFun 9DoF Razor IMU M0 38 | * **[MPU-9250 Breakout Repository](https://github.com/sparkfun/MPU-9250_Breakout)** - Main repository (including hardware files) for the MPU-9250 Breakout. 39 | * **[Hookup Guide](https://learn.sparkfun.com/tutorials/9dof-razor-imu-m0-hookup-guide)** - Basic hookup guide for the SparkFun 9DoF Razor IMU M0, including a [section on using this library](https://learn.sparkfun.com/tutorials/9dof-razor-imu-m0-hookup-guide#using-the-mpu-9250-dmp-arduino-library). 40 | 41 | Products that use this Library 42 | --------------------------------- 43 | 44 | * [SparkFun 9DoF Razor IMU M0 (SEN-14001)](https://www.sparkfun.com/products/14001)- An MPU-9250 development board, which includes an Arduino-compatible SAMD21 processor, LiPo battery charger, and USB interface. 45 | * [SparkFun MPU-9250 Breakout (SEN-13762)](https://www.sparkfun.com/products/13762)- Easily adaptible breakout board for the MPU-9250. 46 | 47 | Version History 48 | --------------- 49 | 50 | 51 | License Information 52 | ------------------- 53 | 54 | This product is _**open source**_! 55 | 56 | Please review the LICENSE.md file for license information. 57 | 58 | If you have any questions or concerns on licensing, please contact techsupport@sparkfun.com. 59 | 60 | Distributed as-is; no warranty is given. 61 | 62 | - Your friends at SparkFun. 63 | -------------------------------------------------------------------------------- /examples/MPU9250_Basic/MPU9250_Basic.ino: -------------------------------------------------------------------------------- 1 | /************************************************************ 2 | MPU9250_Basic 3 | Basic example sketch for MPU-9250 DMP Arduino Library 4 | Jim Lindblom @ SparkFun Electronics 5 | original creation date: November 23, 2016 6 | https://github.com/sparkfun/SparkFun_MPU9250_DMP_Arduino_Library 7 | 8 | This example sketch demonstrates how to initialize the 9 | MPU-9250, and stream its sensor outputs to a serial monitor. 10 | 11 | Development environment specifics: 12 | Arduino IDE 1.6.12 13 | SparkFun 9DoF Razor IMU M0 14 | 15 | Supported Platforms: 16 | - ATSAMD21 (Arduino Zero, SparkFun SAMD21 Breakouts) 17 | *************************************************************/ 18 | #include 19 | 20 | #ifdef defined(SAMD) 21 | #define SerialPort SerialUSB 22 | #else 23 | #define SerialPort Serial 24 | #endif 25 | 26 | MPU9250_DMP imu; 27 | 28 | void setup() 29 | { 30 | SerialPort.begin(115200); 31 | 32 | // Call imu.begin() to verify communication with and 33 | // initialize the MPU-9250 to it's default values. 34 | // Most functions return an error code - INV_SUCCESS (0) 35 | // indicates the IMU was present and successfully set up 36 | if (imu.begin() != INV_SUCCESS) 37 | { 38 | while (1) 39 | { 40 | SerialPort.println("Unable to communicate with MPU-9250"); 41 | SerialPort.println("Check connections, and try again."); 42 | SerialPort.println(); 43 | delay(5000); 44 | } 45 | } 46 | 47 | // Use setSensors to turn on or off MPU-9250 sensors. 48 | // Any of the following defines can be combined: 49 | // INV_XYZ_GYRO, INV_XYZ_ACCEL, INV_XYZ_COMPASS, 50 | // INV_X_GYRO, INV_Y_GYRO, or INV_Z_GYRO 51 | // Enable all sensors: 52 | imu.setSensors(INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_XYZ_COMPASS); 53 | 54 | // Use setGyroFSR() and setAccelFSR() to configure the 55 | // gyroscope and accelerometer full scale ranges. 56 | // Gyro options are +/- 250, 500, 1000, or 2000 dps 57 | imu.setGyroFSR(2000); // Set gyro to 2000 dps 58 | // Accel options are +/- 2, 4, 8, or 16 g 59 | imu.setAccelFSR(2); // Set accel to +/-2g 60 | // Note: the MPU-9250's magnetometer FSR is set at 61 | // +/- 4912 uT (micro-tesla's) 62 | 63 | // setLPF() can be used to set the digital low-pass filter 64 | // of the accelerometer and gyroscope. 65 | // Can be any of the following: 188, 98, 42, 20, 10, 5 66 | // (values are in Hz). 67 | imu.setLPF(5); // Set LPF corner frequency to 5Hz 68 | 69 | // The sample rate of the accel/gyro can be set using 70 | // setSampleRate. Acceptable values range from 4Hz to 1kHz 71 | imu.setSampleRate(10); // Set sample rate to 10Hz 72 | 73 | // Likewise, the compass (magnetometer) sample rate can be 74 | // set using the setCompassSampleRate() function. 75 | // This value can range between: 1-100Hz 76 | imu.setCompassSampleRate(10); // Set mag rate to 10Hz 77 | } 78 | 79 | void loop() 80 | { 81 | // dataReady() checks to see if new accel/gyro data 82 | // is available. It will return a boolean true or false 83 | // (New magnetometer data cannot be checked, as the library 84 | // runs that sensor in single-conversion mode.) 85 | if ( imu.dataReady() ) 86 | { 87 | // Call update() to update the imu objects sensor data. 88 | // You can specify which sensors to update by combining 89 | // UPDATE_ACCEL, UPDATE_GYRO, UPDATE_COMPASS, and/or 90 | // UPDATE_TEMPERATURE. 91 | // (The update function defaults to accel, gyro, compass, 92 | // so you don't have to specify these values.) 93 | imu.update(UPDATE_ACCEL | UPDATE_GYRO | UPDATE_COMPASS); 94 | printIMUData(); 95 | } 96 | } 97 | 98 | void printIMUData(void) 99 | { 100 | // After calling update() the ax, ay, az, gx, gy, gz, mx, 101 | // my, mz, time, and/or temerature class variables are all 102 | // updated. Access them by placing the object. in front: 103 | 104 | // Use the calcAccel, calcGyro, and calcMag functions to 105 | // convert the raw sensor readings (signed 16-bit values) 106 | // to their respective units. 107 | float accelX = imu.calcAccel(imu.ax); 108 | float accelY = imu.calcAccel(imu.ay); 109 | float accelZ = imu.calcAccel(imu.az); 110 | float gyroX = imu.calcGyro(imu.gx); 111 | float gyroY = imu.calcGyro(imu.gy); 112 | float gyroZ = imu.calcGyro(imu.gz); 113 | float magX = imu.calcMag(imu.mx); 114 | float magY = imu.calcMag(imu.my); 115 | float magZ = imu.calcMag(imu.mz); 116 | 117 | SerialPort.println("Accel: " + String(accelX) + ", " + 118 | String(accelY) + ", " + String(accelZ) + " g"); 119 | SerialPort.println("Gyro: " + String(gyroX) + ", " + 120 | String(gyroY) + ", " + String(gyroZ) + " dps"); 121 | SerialPort.println("Mag: " + String(magX) + ", " + 122 | String(magY) + ", " + String(magZ) + " uT"); 123 | SerialPort.println("Time: " + String(imu.time) + " ms"); 124 | SerialPort.println(); 125 | } 126 | 127 | -------------------------------------------------------------------------------- /examples/MPU9250_Basic_Interrupt/MPU9250_Basic_Interrupt.ino: -------------------------------------------------------------------------------- 1 | /************************************************************ 2 | MPU9250_Basic_Interrupt 3 | Basic interrupt sketch for MPU-9250 DMP Arduino Library 4 | Jim Lindblom @ SparkFun Electronics 5 | original creation date: November 23, 2016 6 | https://github.com/sparkfun/SparkFun_MPU9250_DMP_Arduino_Library 7 | 8 | This example sketch demonstrates how to initialize the 9 | MPU-9250, and stream its sensor outputs to a serial monitor. 10 | It uses the MPU-9250's interrupt output to indicate when 11 | new data is ready. 12 | 13 | Development environment specifics: 14 | Arduino IDE 1.6.12 15 | SparkFun 9DoF Razor IMU M0 (interrupt on pin 4) 16 | 17 | Supported Platforms: 18 | - ATSAMD21 (Arduino Zero, SparkFun SAMD21 Breakouts) 19 | *************************************************************/ 20 | #include 21 | 22 | #ifdef defined(SAMD) 23 | #define SerialPort SerialUSB 24 | #else 25 | #define SerialPort Serial 26 | #endif 27 | 28 | #define INTERRUPT_PIN 4 29 | 30 | MPU9250_DMP imu; 31 | 32 | void setup() 33 | { 34 | pinMode(INTERRUPT_PIN, INPUT_PULLUP); 35 | SerialPort.begin(115200); 36 | 37 | if (imu.begin() != INV_SUCCESS) 38 | { 39 | while (1) 40 | { 41 | SerialPort.println("Unable to communicate with MPU-9250"); 42 | SerialPort.println("Check connections, and try again."); 43 | SerialPort.println(); 44 | delay(5000); 45 | } 46 | } 47 | 48 | // Enable all sensors, and set sample rates to 4Hz. 49 | // (Slow so we can see the interrupt work.) 50 | imu.setSensors(INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_XYZ_COMPASS); 51 | imu.setSampleRate(4); // Set accel/gyro sample rate to 4Hz 52 | imu.setCompassSampleRate(4); // Set mag rate to 4Hz 53 | 54 | // Use enableInterrupt() to configure the MPU-9250's 55 | // interrupt output as a "data ready" indicator. 56 | imu.enableInterrupt(); 57 | 58 | // The interrupt level can either be active-high or low. 59 | // Configure as active-low, since we'll be using the pin's 60 | // internal pull-up resistor. 61 | // Options are INT_ACTIVE_LOW or INT_ACTIVE_HIGH 62 | imu.setIntLevel(INT_ACTIVE_LOW); 63 | 64 | // The interrupt can be set to latch until data has 65 | // been read, or to work as a 50us pulse. 66 | // Use latching method -- we'll read from the sensor 67 | // as soon as we see the pin go LOW. 68 | // Options are INT_LATCHED or INT_50US_PULSE 69 | imu.setIntLatched(INT_LATCHED); 70 | } 71 | 72 | void loop() 73 | { 74 | // The interrupt pin is pulled up using an internal pullup 75 | // resistor, and the MPU-9250 is configured to trigger 76 | // the input LOW. 77 | if ( digitalRead(INTERRUPT_PIN) == LOW ) 78 | { 79 | // Call update() to update the imu objects sensor data. 80 | imu.update(UPDATE_ACCEL | UPDATE_GYRO | UPDATE_COMPASS); 81 | printIMUData(); 82 | } 83 | } 84 | 85 | void printIMUData(void) 86 | { 87 | // After calling update() the ax, ay, az, gx, gy, gz, mx, 88 | // my, mz, time, and/or temerature class variables are all 89 | // updated. Access them by placing the object. in front: 90 | 91 | // Use the calcAccel, calcGyro, and calcMag functions to 92 | // convert the raw sensor readings (signed 16-bit values) 93 | // to their respective units. 94 | float accelX = imu.calcAccel(imu.ax); 95 | float accelY = imu.calcAccel(imu.ay); 96 | float accelZ = imu.calcAccel(imu.az); 97 | float gyroX = imu.calcGyro(imu.gx); 98 | float gyroY = imu.calcGyro(imu.gy); 99 | float gyroZ = imu.calcGyro(imu.gz); 100 | float magX = imu.calcMag(imu.mx); 101 | float magY = imu.calcMag(imu.my); 102 | float magZ = imu.calcMag(imu.mz); 103 | 104 | SerialPort.println("Accel: " + String(accelX) + ", " + 105 | String(accelY) + ", " + String(accelZ) + " g"); 106 | SerialPort.println("Gyro: " + String(gyroX) + ", " + 107 | String(gyroY) + ", " + String(gyroZ) + " dps"); 108 | SerialPort.println("Mag: " + String(magX) + ", " + 109 | String(magY) + ", " + String(magZ) + " uT"); 110 | SerialPort.println("Time: " + String(imu.time) + " ms"); 111 | SerialPort.println(); 112 | } 113 | 114 | -------------------------------------------------------------------------------- /examples/MPU9250_DMP_Gyro_Cal/MPU9250_DMP_Gyro_Cal.ino: -------------------------------------------------------------------------------- 1 | /************************************************************ 2 | MPU9250_DMP_Gyro_Cal 3 | Gyro calibration example for MPU-9250 DMP Arduino Library 4 | Jim Lindblom @ SparkFun Electronics 5 | original creation date: November 23, 2016 6 | https://github.com/sparkfun/SparkFun_MPU9250_DMP_Arduino_Library 7 | 8 | This example sketch demonstrates how to use the MPU-9250's 9 | digital motion processor (DMP) to calibrate the gyroscope. 10 | After eight seconds of no motion, the DMP will compute 11 | gyro biases and subtract them. 12 | 13 | Development environment specifics: 14 | Arduino IDE 1.6.12 15 | SparkFun 9DoF Razor IMU M0 16 | 17 | Supported Platforms: 18 | - ATSAMD21 (Arduino Zero, SparkFun SAMD21 Breakouts) 19 | *************************************************************/ 20 | #include 21 | 22 | #ifdef defined(SAMD) 23 | #define SerialPort SerialUSB 24 | #else 25 | #define SerialPort Serial 26 | #endif 27 | 28 | MPU9250_DMP imu; 29 | 30 | void setup() 31 | { 32 | SerialPort.begin(115200); 33 | 34 | // Call imu.begin() to verify communication and initialize 35 | if (imu.begin() != INV_SUCCESS) 36 | { 37 | while (1) 38 | { 39 | SerialPort.println("Unable to communicate with MPU-9250"); 40 | SerialPort.println("Check connections, and try again."); 41 | SerialPort.println(); 42 | delay(5000); 43 | } 44 | } 45 | 46 | imu.setSensors(INV_XYZ_GYRO); // Enable gyroscope only 47 | imu.setGyroFSR(2000); // Set gyro to 2000 dps 48 | 49 | imu.dmpBegin(DMP_FEATURE_GYRO_CAL | // Enable gyro cal 50 | DMP_FEATURE_SEND_CAL_GYRO,// Send cal'd gyro values 51 | 10); // Set DMP rate to 10 Hz 52 | } 53 | 54 | void loop() 55 | { 56 | // Check for new data in the FIFO 57 | if ( imu.fifoAvailable() ) 58 | { 59 | // Use dmpUpdateFifo to update the ax, gx, mx, etc. values 60 | if ( imu.dmpUpdateFifo() == INV_SUCCESS) 61 | { 62 | printIMUData(); 63 | } 64 | } 65 | } 66 | 67 | void printIMUData(void) 68 | { 69 | // After calling dmpUpdateFifo() the ax, gx, mx, etc. values 70 | // are all updated. 71 | float gyroX = imu.calcGyro(imu.gx); 72 | float gyroY = imu.calcGyro(imu.gy); 73 | float gyroZ = imu.calcGyro(imu.gz); 74 | 75 | SerialPort.println("Gyro: " + String(gyroX) + ", " + 76 | String(gyroY) + ", " + String(gyroZ) + " dps"); 77 | SerialPort.println("Time: " + String(imu.time) + " ms"); 78 | SerialPort.println(); 79 | } 80 | 81 | -------------------------------------------------------------------------------- /examples/MPU9250_DMP_Orientation/MPU9250_DMP_Orientation.ino: -------------------------------------------------------------------------------- 1 | /************************************************************ 2 | MPU9250_DMP_Orientation 3 | Orientation example for MPU-9250 DMP Arduino Library 4 | Jim Lindblom @ SparkFun Electronics 5 | original creation date: November 23, 2016 6 | https://github.com/sparkfun/SparkFun_MPU9250_DMP_Arduino_Library 7 | 8 | Uses the MPU-9250's digital motion processing engine to 9 | determine orientation of the board. 10 | 11 | Development environment specifics: 12 | Arduino IDE 1.6.12 13 | SparkFun 9DoF Razor IMU M0 14 | 15 | Supported Platforms: 16 | - ATSAMD21 (Arduino Zero, SparkFun SAMD21 Breakouts) 17 | *************************************************************/ 18 | #include 19 | 20 | #ifdef defined(SAMD) 21 | #define SerialPort SerialUSB 22 | #else 23 | #define SerialPort Serial 24 | #endif 25 | 26 | MPU9250_DMP imu; 27 | 28 | unsigned long stepCount = 0; 29 | unsigned long stepTime = 0; 30 | unsigned long lastStepCount = 0; 31 | 32 | const signed char orientationMatrix[9] = { 33 | 1, 0, 0, 34 | 0, 1, 0, 35 | 0, 0, 1 36 | }; 37 | unsigned char lastOrient = 0; 38 | 39 | void setup() 40 | { 41 | SerialPort.begin(115200); 42 | 43 | // Call imu.begin() to verify communication and initialize 44 | if (imu.begin() != INV_SUCCESS) 45 | { 46 | while (1) 47 | { 48 | SerialPort.println("Unable to communicate with MPU-9250"); 49 | SerialPort.println("Check connections, and try again."); 50 | SerialPort.println(); 51 | delay(5000); 52 | } 53 | } 54 | 55 | imu.dmpBegin(DMP_FEATURE_ANDROID_ORIENT); 56 | imu.dmpSetOrientation(orientationMatrix); 57 | } 58 | 59 | void loop() 60 | { 61 | if ( imu.fifoAvailable() ) 62 | { 63 | imu.dmpUpdateFifo(); 64 | unsigned char orient = imu.dmpGetOrientation(); 65 | if (orient != lastOrient) 66 | { 67 | switch (orient) 68 | { 69 | case ORIENT_PORTRAIT: 70 | SerialPort.println("Portrait"); 71 | break; 72 | case ORIENT_LANDSCAPE: 73 | SerialPort.println("Landscape"); 74 | break; 75 | case ORIENT_REVERSE_PORTRAIT: 76 | SerialPort.println("Portrait (Reverse)"); 77 | break; 78 | case ORIENT_REVERSE_LANDSCAPE: 79 | SerialPort.println("Landscape (Reverse)"); 80 | break; 81 | } 82 | lastOrient = orient; 83 | } 84 | } 85 | } 86 | 87 | -------------------------------------------------------------------------------- /examples/MPU9250_DMP_Pedometer/MPU9250_DMP_Pedometer.ino: -------------------------------------------------------------------------------- 1 | /************************************************************ 2 | MPU9250_DMP_Pedometer 3 | Pedometer example for MPU-9250 DMP Arduino Library 4 | Jim Lindblom @ SparkFun Electronics 5 | original creation date: November 23, 2016 6 | https://github.com/sparkfun/SparkFun_MPU9250_DMP_Arduino_Library 7 | 8 | The MPU-9250's digital motion processor (DMP) can estimate 9 | steps taken -- effecting a pedometer. 10 | 11 | After uploading the code, try shaking the 9DoF up and 12 | down at a "stepping speed." 13 | 14 | Development environment specifics: 15 | Arduino IDE 1.6.12 16 | SparkFun 9DoF Razor IMU M0 17 | 18 | Supported Platforms: 19 | - ATSAMD21 (Arduino Zero, SparkFun SAMD21 Breakouts) 20 | *************************************************************/ 21 | #include 22 | 23 | #ifdef defined(SAMD) 24 | #define SerialPort SerialUSB 25 | #else 26 | #define SerialPort Serial 27 | #endif 28 | 29 | MPU9250_DMP imu; 30 | 31 | unsigned long stepCount = 0; 32 | unsigned long stepTime = 0; 33 | unsigned long lastStepCount = 0; 34 | 35 | void setup() 36 | { 37 | SerialPort.begin(115200); 38 | 39 | // Call imu.begin() to verify communication and initialize 40 | if (imu.begin() != INV_SUCCESS) 41 | { 42 | while (1) 43 | { 44 | SerialPort.println("Unable to communicate with MPU-9250"); 45 | SerialPort.println("Check connections, and try again."); 46 | SerialPort.println(); 47 | delay(5000); 48 | } 49 | } 50 | 51 | imu.dmpBegin(DMP_FEATURE_PEDOMETER); 52 | imu.dmpSetPedometerSteps(stepCount); 53 | imu.dmpSetPedometerTime(stepTime); 54 | } 55 | 56 | void loop() 57 | { 58 | stepCount = imu.dmpGetPedometerSteps(); 59 | stepTime = imu.dmpGetPedometerTime(); 60 | 61 | if (stepCount != lastStepCount) 62 | { 63 | lastStepCount = stepCount; 64 | SerialPort.print("Walked " + String(stepCount) + 65 | " steps"); 66 | SerialPort.println(" (" + 67 | String((float)stepTime / 1000.0) + " s)"); 68 | } 69 | } 70 | 71 | -------------------------------------------------------------------------------- /examples/MPU9250_DMP_Quaternion/MPU9250_DMP_Quaternion.ino: -------------------------------------------------------------------------------- 1 | /************************************************************ 2 | MPU9250_DMP_Quaternion 3 | Quaternion example for MPU-9250 DMP Arduino Library 4 | Jim Lindblom @ SparkFun Electronics 5 | original creation date: November 23, 2016 6 | https://github.com/sparkfun/SparkFun_MPU9250_DMP_Arduino_Library 7 | 8 | The MPU-9250's digital motion processor (DMP) can calculate 9 | four unit quaternions, which can be used to represent the 10 | rotation of an object. 11 | 12 | This exmaple demonstrates how to configure the DMP to 13 | calculate quaternions, and prints them out to the serial 14 | monitor. It also calculates pitch, roll, and yaw from those 15 | values. 16 | 17 | Development environment specifics: 18 | Arduino IDE 1.6.12 19 | SparkFun 9DoF Razor IMU M0 20 | 21 | Supported Platforms: 22 | - ATSAMD21 (Arduino Zero, SparkFun SAMD21 Breakouts) 23 | *************************************************************/ 24 | #include 25 | 26 | #ifdef defined(SAMD) 27 | #define SerialPort SerialUSB 28 | #else 29 | #define SerialPort Serial 30 | #endif 31 | 32 | MPU9250_DMP imu; 33 | 34 | void setup() 35 | { 36 | SerialPort.begin(115200); 37 | 38 | // Call imu.begin() to verify communication and initialize 39 | if (imu.begin() != INV_SUCCESS) 40 | { 41 | while (1) 42 | { 43 | SerialPort.println("Unable to communicate with MPU-9250"); 44 | SerialPort.println("Check connections, and try again."); 45 | SerialPort.println(); 46 | delay(5000); 47 | } 48 | } 49 | 50 | imu.dmpBegin(DMP_FEATURE_6X_LP_QUAT | // Enable 6-axis quat 51 | DMP_FEATURE_GYRO_CAL, // Use gyro calibration 52 | 10); // Set DMP FIFO rate to 10 Hz 53 | // DMP_FEATURE_LP_QUAT can also be used. It uses the 54 | // accelerometer in low-power mode to estimate quat's. 55 | // DMP_FEATURE_LP_QUAT and 6X_LP_QUAT are mutually exclusive 56 | } 57 | 58 | void loop() 59 | { 60 | // Check for new data in the FIFO 61 | if ( imu.fifoAvailable() ) 62 | { 63 | // Use dmpUpdateFifo to update the ax, gx, mx, etc. values 64 | if ( imu.dmpUpdateFifo() == INV_SUCCESS) 65 | { 66 | // computeEulerAngles can be used -- after updating the 67 | // quaternion values -- to estimate roll, pitch, and yaw 68 | imu.computeEulerAngles(); 69 | printIMUData(); 70 | } 71 | } 72 | } 73 | 74 | void printIMUData(void) 75 | { 76 | // After calling dmpUpdateFifo() the ax, gx, mx, etc. values 77 | // are all updated. 78 | // Quaternion values are, by default, stored in Q30 long 79 | // format. calcQuat turns them into a float between -1 and 1 80 | float q0 = imu.calcQuat(imu.qw); 81 | float q1 = imu.calcQuat(imu.qx); 82 | float q2 = imu.calcQuat(imu.qy); 83 | float q3 = imu.calcQuat(imu.qz); 84 | 85 | SerialPort.println("Q: " + String(q0, 4) + ", " + 86 | String(q1, 4) + ", " + String(q2, 4) + 87 | ", " + String(q3, 4)); 88 | SerialPort.println("R/P/Y: " + String(imu.roll) + ", " 89 | + String(imu.pitch) + ", " + String(imu.yaw)); 90 | SerialPort.println("Time: " + String(imu.time) + " ms"); 91 | SerialPort.println(); 92 | } 93 | 94 | -------------------------------------------------------------------------------- /examples/MPU9250_DMP_Tap/MPU9250_DMP_Tap.ino: -------------------------------------------------------------------------------- 1 | /************************************************************ 2 | MPU9250_DMP_Tap 3 | Tap-detection example for MPU-9250 DMP Arduino Library 4 | Jim Lindblom @ SparkFun Electronics 5 | original creation date: November 23, 2016 6 | https://github.com/sparkfun/SparkFun_MPU9250_DMP_Arduino_Library 7 | 8 | The MPU-9250's digital motion processor (DMP) can monitor for 9 | single or double tap events on any of the three accelerometer 10 | axes. 11 | 12 | This example turns tap-detection on in the z-axis. Try to 13 | tap it to the max count of 8! 14 | 15 | Development environment specifics: 16 | Arduino IDE 1.6.12 17 | SparkFun 9DoF Razor IMU M0 18 | 19 | Supported Platforms: 20 | - ATSAMD21 (Arduino Zero, SparkFun SAMD21 Breakouts) 21 | *************************************************************/ 22 | #include 23 | 24 | #ifdef defined(SAMD) 25 | #define SerialPort SerialUSB 26 | #else 27 | #define SerialPort Serial 28 | #endif 29 | 30 | MPU9250_DMP imu; 31 | 32 | void setup() 33 | { 34 | SerialPort.begin(115200); 35 | 36 | // Call imu.begin() to verify communication and initialize 37 | if (imu.begin() != INV_SUCCESS) 38 | { 39 | while (1) 40 | { 41 | SerialPort.println("Unable to communicate with MPU-9250"); 42 | SerialPort.println("Check connections, and try again."); 43 | SerialPort.println(); 44 | delay(5000); 45 | } 46 | } 47 | 48 | // Enable tap detection in the DMP. Set FIFO sample rate to 10Hz. 49 | imu.dmpBegin(DMP_FEATURE_TAP, 10); 50 | // dmpSetTap parameters, in order, are: 51 | // x threshold: 1-1600 (0 to disable) 52 | // y threshold: 1-1600 (0 to disable) 53 | // z threshold: 1-1600 (0 to disable) 54 | // (Threshold units are mg/ms) 55 | // taps: Minimum number of taps needed for interrupt (1-4) 56 | // tap time: milliseconds between valid taps 57 | // tap time multi: max milliseconds between multi-taps 58 | unsigned short xThresh = 0; // Disable x-axis tap 59 | unsigned short yThresh = 0; // Disable y-axis tap 60 | unsigned short zThresh = 100; // Set z-axis tap thresh to 100 mg/ms 61 | unsigned char taps = 1; // Set minimum taps to 1 62 | unsigned short tapTime = 100; // Set tap time to 100ms 63 | unsigned short tapMulti = 1000;// Set multi-tap time to 1s 64 | imu.dmpSetTap(xThresh, yThresh, zThresh, taps, tapTime, tapMulti); 65 | } 66 | 67 | void loop() 68 | { 69 | // Check for new data in the FIFO 70 | if ( imu.fifoAvailable() ) 71 | { 72 | // DMP FIFO must be updated in order to update tap data 73 | imu.dmpUpdateFifo(); 74 | // Check for new tap data by polling tapAvailable 75 | if ( imu.tapAvailable() ) 76 | { 77 | // If a new tap happened, get the direction and count 78 | // by reading getTapDir and getTapCount 79 | unsigned char tapDir = imu.getTapDir(); 80 | unsigned char tapCnt = imu.getTapCount(); 81 | switch (tapDir) 82 | { 83 | case TAP_X_UP: 84 | SerialPort.print("Tap X+ "); 85 | break; 86 | case TAP_X_DOWN: 87 | SerialPort.print("Tap X- "); 88 | break; 89 | case TAP_Y_UP: 90 | SerialPort.print("Tap Y+ "); 91 | break; 92 | case TAP_Y_DOWN: 93 | SerialPort.print("Tap Y- "); 94 | break; 95 | case TAP_Z_UP: 96 | SerialPort.print("Tap Z+ "); 97 | break; 98 | case TAP_Z_DOWN: 99 | SerialPort.print("Tap Z- "); 100 | break; 101 | } 102 | SerialPort.println(tapCnt); 103 | } 104 | } 105 | } 106 | 107 | -------------------------------------------------------------------------------- /examples/MPU9250_FIFO_Basic/MPU9250_FIFO_Basic.ino: -------------------------------------------------------------------------------- 1 | /************************************************************ 2 | MPU9250_FIFO_Basic 3 | Basic example sketch for MPU-9250 DMP Arduino Library 4 | Jim Lindblom @ SparkFun Electronics 5 | original creation date: November 23, 2016 6 | https://github.com/sparkfun/SparkFun_MPU9250_DMP_Arduino_Library 7 | 8 | This example sketch demonstrates how to use the MPU-9250's 9 | 512 byte first-in, first-out (FIFO) buffer. The FIFO can be 10 | set to store either accelerometer and/or gyroscope (not the 11 | magnetometer, though :( ). 12 | 13 | Development environment specifics: 14 | Arduino IDE 1.6.12 15 | SparkFun 9DoF Razor IMU M0 16 | 17 | Supported Platforms: 18 | - ATSAMD21 (Arduino Zero, SparkFun SAMD21 Breakouts) 19 | *************************************************************/ 20 | #include 21 | 22 | #ifdef defined(SAMD) 23 | #define SerialPort SerialUSB 24 | #else 25 | #define SerialPort Serial 26 | #endif 27 | 28 | MPU9250_DMP imu; 29 | 30 | void setup() 31 | { 32 | SerialPort.begin(115200); 33 | 34 | // Call imu.begin() to verify communication with and 35 | // initialize the MPU-9250 to it's default values. 36 | // Most functions return an error code - INV_SUCCESS (0) 37 | // indicates the IMU was present and successfully set up 38 | if (imu.begin() != INV_SUCCESS) 39 | { 40 | while (1) 41 | { 42 | SerialPort.println("Unable to communicate with MPU-9250"); 43 | SerialPort.println("Check connections, and try again."); 44 | SerialPort.println(); 45 | delay(5000); 46 | } 47 | } 48 | 49 | // The sample rate of the accel/gyro can be set using 50 | // setSampleRate. Acceptable values range from 4Hz to 1kHz 51 | imu.setSampleRate(100); // Set sample rate to 100Hz 52 | 53 | // Use configureFifo to set which sensors should be stored 54 | // in the buffer. 55 | // Parameter to this function can be: INV_XYZ_GYRO, 56 | // INV_XYZ_ACCEL, INV_X_GYRO, INV_Y_GYRO, or INV_Z_GYRO 57 | imu.configureFifo(INV_XYZ_GYRO |INV_XYZ_ACCEL); 58 | } 59 | 60 | void loop() 61 | { 62 | // fifoAvailable returns the number of bytes in the FIFO 63 | // The FIFO is 512 bytes max. We'll read when it reaches 64 | // half of that. 65 | if ( imu.fifoAvailable() >= 256) 66 | { 67 | // Then read while there is data in the FIFO 68 | while ( imu.fifoAvailable() > 0) 69 | { 70 | // Call updateFifo to update ax, ay, az, gx, gy, and/or gz 71 | if ( imu.updateFifo() == INV_SUCCESS) 72 | { 73 | printIMUData(); 74 | } 75 | } 76 | } 77 | } 78 | 79 | void printIMUData(void) 80 | { 81 | // After calling update() the ax, ay, az, gx, gy, gz, mx, 82 | // my, mz, time, and/or temerature class variables are all 83 | // updated. Access them by placing the object. in front: 84 | 85 | // Use the calcAccel, calcGyro, and calcMag functions to 86 | // convert the raw sensor readings (signed 16-bit values) 87 | // to their respective units. 88 | float accelX = imu.calcAccel(imu.ax); 89 | float accelY = imu.calcAccel(imu.ay); 90 | float accelZ = imu.calcAccel(imu.az); 91 | float gyroX = imu.calcGyro(imu.gx); 92 | float gyroY = imu.calcGyro(imu.gy); 93 | float gyroZ = imu.calcGyro(imu.gz); 94 | 95 | SerialPort.println("Accel: " + String(accelX) + ", " + 96 | String(accelY) + ", " + String(accelZ) + " g"); 97 | SerialPort.println("Gyro: " + String(gyroX) + ", " + 98 | String(gyroY) + ", " + String(gyroZ) + " dps"); 99 | SerialPort.println("Time: " + String(imu.time) + " ms"); 100 | SerialPort.println(); 101 | } 102 | -------------------------------------------------------------------------------- /keywords.txt: -------------------------------------------------------------------------------- 1 | ################################################################################ 2 | # Syntax Coloring Map for SparkFun MPU-9250 Arduino Library # 3 | ################################################################################ 4 | 5 | ################################################################################ 6 | # Datatypes (KEYWORD1) 7 | ################################################################################ 8 | SparkFunMPU9250-DMP KEYWORD1 9 | MPU9250_DMP KEYWORD1 10 | ax KEYWORD1 11 | ay KEYWORD1 12 | az KEYWORD1 13 | gx KEYWORD1 14 | gy KEYWORD1 15 | gz KEYWORD1 16 | mx KEYWORD1 17 | my KEYWORD1 18 | mz KEYWORD1 19 | qw KEYWORD1 20 | qx KEYWORD1 21 | qy KEYWORD1 22 | qz KEYWORD1 23 | temperature KEYWORD1 24 | time KEYWORD1 25 | pitch KEYWORD1 26 | roll KEYWORD1 27 | yaw KEYWORD1 28 | heading KEYWORD1 29 | 30 | ################################################################################ 31 | # Methods and Functions (KEYWORD2) 32 | ################################################################################ 33 | begin KEYWORD2 34 | setSensors KEYWORD2 35 | setGyroFSR KEYWORD2 36 | getGyroFSR KEYWORD2 37 | getGyroSens KEYWORD2 38 | setAccelFSR KEYWORD2 39 | getAccelFSR KEYWORD2 40 | getAccelSens KEYWORD2 41 | getMagFSR KEYWORD2 42 | getMagSens KEYWORD2 43 | setLPF KEYWORD2 44 | getLPF KEYWORD2 45 | setSampleRate KEYWORD2 46 | getSampleRate KEYWORD2 47 | setCompassSampleRate KEYWORD2 48 | getCompassSampleRate KEYWORD2 49 | lowPowerAccel KEYWORD2 50 | dataReady KEYWORD2 51 | update KEYWORD2 52 | updateAccel KEYWORD2 53 | updateGyro KEYWORD2 54 | updateCompass KEYWORD2 55 | updateTemperature KEYWORD2 56 | getFifoConfig KEYWORD2 57 | configureFifo KEYWORD2 58 | resetFifo KEYWORD2 59 | fifoAvailable KEYWORD2 60 | updateFifo KEYWORD2 61 | selfTest KEYWORD2 62 | enableInterrupt KEYWORD2 63 | setIntLevel KEYWORD2 64 | setIntLatched KEYWORD2 65 | getIntStatus KEYWORD2 66 | dmpBegin KEYWORD2 67 | dmpLoad KEYWORD2 68 | dmpGetFifoRate KEYWORD2 69 | dmpSetFifoRate KEYWORD2 70 | dmpUpdateFifo KEYWORD2 71 | dmpEnableFeatures KEYWORD2 72 | dmpGetEnabledFeatures KEYWORD2 73 | dmpSetInterruptMode KEYWORD2 74 | dmpSetGyroBias KEYWORD2 75 | dmpSetAccelBias KEYWORD2 76 | dmpSetTap KEYWORD2 77 | tapAvailable KEYWORD2 78 | getTapDir KEYWORD2 79 | getTapCount KEYWORD2 80 | dmpSetOrientation KEYWORD2 81 | dmpGetOrientation KEYWORD2 82 | dmpEnable3Quat KEYWORD2 83 | dmpEnable6Quat KEYWORD2 84 | dmpGetPedometerSteps KEYWORD2 85 | dmpSetPedometerSteps KEYWORD2 86 | dmpGetPedometerTime KEYWORD2 87 | dmpSetPedometerTime KEYWORD2 88 | calcAccel KEYWORD2 89 | calcGyro KEYWORD2 90 | calcMag KEYWORD2 91 | calcQuat KEYWORD2 92 | qToFloat KEYWORD2 93 | computeEulerAngles KEYWORD2 94 | computeCompassHeading KEYWORD2 95 | 96 | ################################################################################ 97 | # Constants (LITERAL1) 98 | ################################################################################ 99 | INV_SUCCESS LITERAL1 100 | INV_XYZ_GYRO LITERAL1 101 | INV_XYZ_ACCEL LITERAL1 102 | INV_XYZ_COMPASS LITERAL1 103 | INV_X_GYRO LITERAL1 104 | INV_Y_GYRO LITERAL1 105 | INV_Z_GYRO LITERAL1 106 | UPDATE_ACCEL LITERAL1 107 | UPDATE_GYRO LITERAL1 108 | UPDATE_COMPASS LITERAL1 109 | UPDATE_TEMP LITERAL1 110 | FIFO_BUFFER_SIZE LITERAL1 111 | INT_ACTIVE_LOW LITERAL1 112 | INT_ACTIVE_HIGH LITERAL1 113 | INT_LATCHED LITERAL1 114 | INT_50US_PULSE LITERAL1 115 | DMP_FEATURE_TAP LITERAL1 116 | DMP_FEATURE_ANDROID_ORIENT LITERAL1 117 | DMP_FEATURE_LP_QUAT LITERAL1 118 | DMP_FEATURE_6X_LP_QUAT LITERAL1 119 | DMP_FEATURE_GYRO_CAL LITERAL1 120 | DMP_FEATURE_SEND_RAW_ACCEL LITERAL1 121 | DMP_FEATURE_SEND_RAW_GYRO LITERAL1 122 | DMP_FEATURE_SEND_CAL_GYRO LITERAL1 123 | DMP_FEATURE_PEDOMETER LITERAL1 124 | ORIENT_PORTRAIT LITERAL1 125 | ORIENT_LANDSCAPE LITERAL1 126 | ORIENT_REVERSE_PORTRAIT LITERAL1 127 | ORIENT_REVERSE_LANDSCAPE LITERAL1 128 | TAP_X_UP LITERAL1 129 | TAP_X_DOWN LITERAL1 130 | TAP_Y_UP LITERAL1 131 | TAP_Y_DOWN LITERAL1 132 | TAP_Z_UP LITERAL1 133 | TAP_Z_DOWN LITERAL1 -------------------------------------------------------------------------------- /library.properties: -------------------------------------------------------------------------------- 1 | name=SparkFun MPU-9250 Digital Motion Processing (DMP) Arduino Library 2 | version=1.0.0 3 | author=SparkFun Electronics 4 | maintainer=SparkFun Electronics 5 | sentence=Driver for InvenSense's MPU-9250 9-DOF IMU (3-axis gyroscope, 3-axis accelerometer & 3-axis magnetometer) 6 | paragraph=The MPU-9250 is a system-in-package featuring acceleration full-scales of ±2 / ±4 / ±8 / ±16 (g), rotational full-scales of ±250 / ±500 / ±1000 / ±2000 (°/sec) and a magnetic field full scale of ±4800 µT. The MPU-9250 includes an I2C serial bus interface that supports speeds up to 400 kHz. 7 | category=Sensors 8 | url=https://github.com/sparkfun/MPU-9250_Breakout 9 | architectures=samd, esp32 10 | -------------------------------------------------------------------------------- /src/MPU9250_RegisterMap.h: -------------------------------------------------------------------------------- 1 | /* 2 | */ 3 | #ifndef _MPU9250_REGISTER_MAP_H_ 4 | #define _MPU9250_REGISTER_MAP_H_ 5 | 6 | enum mpu9250_register { 7 | MPU9250_SELF_TEST_X_GYRO = 0x00, 8 | MPU9250_SELF_TEST_Y_GYRO = 0x01, 9 | MPU9250_SELF_TEST_Z_GYRO = 0x02, 10 | MPU9250_SELF_TEST_X_ACCEL = 0x0D, 11 | MPU9250_SELF_TEST_Y_ACCEL = 0x0E, 12 | MPU9250_SELF_TEST_Z_ACCEL = 0x0F, 13 | MPU9250_XG_OFFSET_H = 0x13, 14 | MPU9250_XG_OFFSET_L = 0x14, 15 | MPU9250_YG_OFFSET_H = 0x15, 16 | MPU9250_YG_OFFSET_L = 0x16, 17 | MPU9250_ZG_OFFSET_H = 0x17, 18 | MPU9250_ZG_OFFSET_L = 0x18, 19 | MPU9250_SMPLRT_DIV = 0x19, 20 | MPU9250_CONFIG = 0x1A, 21 | MPU9250_GYRO_CONFIG = 0x1B, 22 | MPU9250_ACCEL_CONFIG = 0x1C, 23 | MPU9250_ACCEL_CONFIG_2 = 0x1D, 24 | MPU9250_LP_ACCEL_ODR = 0x1E, 25 | MPU9250_WOM_THR = 0x1F, 26 | MPU9250_FIFO_EN = 0x23, 27 | MPU9250_I2C_MST_CTRL = 0x24, 28 | MPU9250_I2C_SLV0_ADDR = 0x25, 29 | MPU9250_I2C_SLV0_REG = 0x26, 30 | MPU9250_I2C_SLV0_CTRL = 0x27, 31 | MPU9250_I2C_SLV1_ADDR = 0x28, 32 | MPU9250_I2C_SLV1_REG = 0x29, 33 | MPU9250_I2C_SLV1_CTRL = 0x2A, 34 | MPU9250_I2C_SLV2_ADDR = 0x2B, 35 | MPU9250_I2C_SLV2_REG = 0x2C, 36 | MPU9250_I2C_SLV2_CTRL = 0x2D, 37 | MPU9250_I2C_SLV3_ADDR = 0x2E, 38 | MPU9250_I2C_SLV3_REG = 0x2F, 39 | MPU9250_I2C_SLV3_CTRL = 0x30, 40 | MPU9250_I2C_SLV4_ADDR = 0x31, 41 | MPU9250_I2C_SLV4_REG = 0x32, 42 | MPU9250_I2C_SLV4_DO = 0x33, 43 | MPU9250_I2C_SLV4_CTRL = 0x34, 44 | MPU9250_I2C_SLV4_DI = 0x35, 45 | MPU9250_I2C_MST_STATUS = 0x36, 46 | MPU9250_INT_PIN_CFG = 0x37, 47 | MPU9250_INT_ENABLE = 0x38, 48 | MPU9250_INT_STATUS = 0x3A, 49 | MPU9250_ACCEL_XOUT_H = 0x3B, 50 | MPU9250_ACCEL_XOUT_L = 0x3C, 51 | MPU9250_ACCEL_YOUT_H = 0x3D, 52 | MPU9250_ACCEL_YOUT_L = 0x3E, 53 | MPU9250_ACCEL_ZOUT_H = 0x3F, 54 | MPU9250_ACCEL_ZOUT_L = 0x40, 55 | MPU9250_TEMP_OUT_H = 0x41, 56 | MPU9250_TEMP_OUT_L = 0x42, 57 | MPU9250_GYRO_XOUT_H = 0x43, 58 | MPU9250_GYRO_XOUT_L = 0x44, 59 | MPU9250_GYRO_YOUT_H = 0x45, 60 | MPU9250_GYRO_YOUT_L = 0x46, 61 | MPU9250_GYRO_ZOUT_H = 0x47, 62 | MPU9250_GYRO_ZOUT_L = 0x48, 63 | MPU9250_EXT_SENS_DATA_00 = 0x49, 64 | MPU9250_EXT_SENS_DATA_01 = 0x4A, 65 | MPU9250_EXT_SENS_DATA_02 = 0x4B, 66 | MPU9250_EXT_SENS_DATA_03 = 0x4C, 67 | MPU9250_EXT_SENS_DATA_04 = 0x4D, 68 | MPU9250_EXT_SENS_DATA_05 = 0x4E, 69 | MPU9250_EXT_SENS_DATA_06 = 0x4F, 70 | MPU9250_EXT_SENS_DATA_07 = 0x50, 71 | MPU9250_EXT_SENS_DATA_08 = 0x51, 72 | MPU9250_EXT_SENS_DATA_09 = 0x52, 73 | MPU9250_EXT_SENS_DATA_10 = 0x53, 74 | MPU9250_EXT_SENS_DATA_11 = 0x54, 75 | MPU9250_EXT_SENS_DATA_12 = 0x55, 76 | MPU9250_EXT_SENS_DATA_13 = 0x56, 77 | MPU9250_EXT_SENS_DATA_14 = 0x57, 78 | MPU9250_EXT_SENS_DATA_15 = 0x58, 79 | MPU9250_EXT_SENS_DATA_16 = 0x59, 80 | MPU9250_EXT_SENS_DATA_17 = 0x5A, 81 | MPU9250_EXT_SENS_DATA_18 = 0x5B, 82 | MPU9250_EXT_SENS_DATA_19 = 0x5C, 83 | MPU9250_EXT_SENS_DATA_20 = 0x5D, 84 | MPU9250_EXT_SENS_DATA_21 = 0x5E, 85 | MPU9250_EXT_SENS_DATA_22 = 0x5F, 86 | MPU9250_EXT_SENS_DATA_23 = 0x60, 87 | MPU9250_I2C_SLV0_DO = 0x63, 88 | MPU9250_I2C_SLV1_DO = 0x64, 89 | MPU9250_I2C_SLV2_DO = 0x65, 90 | MPU9250_I2C_SLV3_DO = 0x66, 91 | MPU9250_I2C_MST_DELAY_CTRL =0x67, 92 | MPU9250_SIGNAL_PATH_RESET = 0x68, 93 | MPU9250_MOT_DETECT_CTRL = 0x69, 94 | MPU9250_USER_CTRL = 0x6A, 95 | MPU9250_PWR_MGMT_1 = 0x6B, 96 | MPU9250_PWR_MGMT_2 = 0x6C, 97 | MPU9250_FIFO_COUNTH = 0x72, 98 | MPU9250_FIFO_COUNTL = 0x73, 99 | MPU9250_FIFO_R_W = 0x74, 100 | MPU9250_WHO_AM_I = 0x75, 101 | MPU9250_XA_OFFSET_H = 0x77, 102 | MPU9250_XA_OFFSET_L = 0x78, 103 | MPU9250_YA_OFFSET_H = 0x7A, 104 | MPU9250_YA_OFFSET_L = 0x7B, 105 | MPU9250_ZA_OFFSET_H = 0x7D, 106 | MPU9250_ZA_OFFSET_L = 0x7E 107 | }; 108 | 109 | enum interrupt_status_bits { 110 | INT_STATUS_RAW_DATA_RDY_INT = 0, 111 | INT_STATUS_FSYNC_INT = 3, 112 | INT_STATUS_FIFO_OVERFLOW_INT = 4, 113 | INT_STATUS_WOM_INT = 6, 114 | }; 115 | 116 | enum gyro_config_bits { 117 | GYRO_CONFIG_FCHOICE_B = 0, 118 | GYRO_CONFIG_GYRO_FS_SEL = 3, 119 | GYRO_CONFIG_ZGYRO_CTEN = 5, 120 | GYRO_CONFIG_YGYRO_CTEN = 6, 121 | GYRO_CONFIG_XGYRO_CTEN = 7, 122 | }; 123 | #define MPU9250_GYRO_FS_SEL_MASK 0x3 124 | #define MPU9250_GYRO_FCHOICE_MASK 0x3 125 | 126 | enum accel_config_bit { 127 | ACCEL_CONFIG_ACCEL_FS_SEL = 3, 128 | ACCEL_CONFIG_AZ_ST_EN = 5, 129 | ACCEL_CONFIG_AY_ST_EN = 6, 130 | ACCEL_CONFIG_AX_ST_EN = 7, 131 | }; 132 | #define MPU9250_ACCEL_FS_SEL_MASK 0x3 133 | 134 | enum accel_config_2_bits { 135 | ACCEL_CONFIG_2_A_DLPFCFG = 0, 136 | ACCEL_CONFIG_2_ACCEL_FCHOICE_B = 3, 137 | }; 138 | 139 | enum pwr_mgmt_1_bits { 140 | PWR_MGMT_1_CLKSEL = 0, 141 | PWR_MGMT_1_PD_PTAT = 3, 142 | PWR_MGMT_1_GYRO_STANDBY = 4, 143 | PWR_MGMT_1_CYCLE = 5, 144 | PWR_MGMT_1_SLEEP = 6, 145 | PWR_MGMT_1_H_RESET = 7 146 | }; 147 | 148 | enum pwr_mgmt_2_bits { 149 | PWR_MGMT_2_DISABLE_ZG = 0, 150 | PWR_MGMT_2_DISABLE_YG = 1, 151 | PWR_MGMT_2_DISABLE_XG = 2, 152 | PWR_MGMT_2_DISABLE_ZA = 3, 153 | PWR_MGMT_2_DISABLE_YA = 4, 154 | PWR_MGMT_2_DISABLE_XA = 5, 155 | }; 156 | 157 | enum int_enable_bits { 158 | INT_ENABLE_RAW_RDY_EN = 0, 159 | INT_ENABLE_FSYNC_INT_EN = 3, 160 | INT_ENABLE_FIFO_OVERFLOW_EN = 4, 161 | INT_ENABLE_WOM_EN = 6, 162 | }; 163 | 164 | enum int_pin_cfg_bits { 165 | INT_PIN_CFG_BYPASS_EN = 1, 166 | INT_PIN_CFG_FSYNC_INT_MODE_EN = 2, 167 | INT_PIN_CFG_ACTL_FSYNC = 3, 168 | INT_PIN_CFG_INT_ANYRD_2CLEAR = 4, 169 | INT_PIN_CFG_LATCH_INT_EN = 5, 170 | INT_PIN_CFG_OPEN = 6, 171 | INT_PIN_CFG_ACTL = 7, 172 | }; 173 | #define INT_PIN_CFG_INT_MASK 0xF0 174 | 175 | #define MPU9250_WHO_AM_I_RESULT 0x71 176 | 177 | enum ak8963_register { 178 | AK8963_WIA = 0x0, 179 | AK8963_INFO = 0x1, 180 | AK8963_ST1 = 0x2, 181 | AK8963_HXL = 0x3, 182 | AK8963_HXH = 0x4, 183 | AK8963_HYL = 0x5, 184 | AK8963_HYH = 0x6, 185 | AK8963_HZL = 0x7, 186 | AK8963_HZH = 0x8, 187 | AK8963_ST2 = 0x9, 188 | AK8963_CNTL = 0xA, 189 | AK8963_RSV = 0xB, 190 | AK8963_ASTC = 0xC, 191 | AK8963_TS1 = 0xD, 192 | AK8963_TS2 = 0xE, 193 | AK8963_I2CDIS = 0xF, 194 | AK8963_ASAX = 0x10, 195 | AK8963_ASAY = 0x11, 196 | AK8963_ASAZ = 0x12, 197 | }; 198 | #define MAG_CTRL_OP_MODE_MASK 0xF 199 | 200 | #define AK8963_ST1_DRDY_BIT 0 201 | 202 | #define AK8963_WHO_AM_I_RESULT 0x48 203 | 204 | #endif // _MPU9250_REGISTER_MAP_H_ -------------------------------------------------------------------------------- /src/SparkFunMPU9250-DMP.cpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | SparkFunMPU9250-DMP.cpp - MPU-9250 Digital Motion Processor Arduino Library 3 | Jim Lindblom @ SparkFun Electronics 4 | original creation date: November 23, 2016 5 | https://github.com/sparkfun/SparkFun_MPU9250_DMP_Arduino_Library 6 | 7 | This library implements motion processing functions of Invensense's MPU-9250. 8 | It is based on their Emedded MotionDriver 6.12 library. 9 | https://www.invensense.com/developers/software-downloads/ 10 | 11 | Development environment specifics: 12 | Arduino IDE 1.6.12 13 | SparkFun 9DoF Razor IMU M0 14 | 15 | Supported Platforms: 16 | - ATSAMD21 (Arduino Zero, SparkFun SAMD21 Breakouts) 17 | ******************************************************************************/ 18 | #include "SparkFunMPU9250-DMP.h" 19 | #include "MPU9250_RegisterMap.h" 20 | 21 | extern "C" { 22 | #include "util/inv_mpu.h" 23 | } 24 | 25 | static unsigned char mpu9250_orientation; 26 | static unsigned char tap_count; 27 | static unsigned char tap_direction; 28 | static bool _tap_available; 29 | static void orient_cb(unsigned char orient); 30 | static void tap_cb(unsigned char direction, unsigned char count); 31 | 32 | MPU9250_DMP::MPU9250_DMP() 33 | { 34 | _mSense = 6.665f; // Constant - 4915 / 32760 35 | _aSense = 0.0f; // Updated after accel FSR is set 36 | _gSense = 0.0f; // Updated after gyro FSR is set 37 | } 38 | 39 | inv_error_t MPU9250_DMP::begin(void) 40 | { 41 | inv_error_t result; 42 | struct int_param_s int_param; 43 | 44 | Wire.begin(); 45 | 46 | result = mpu_init(&int_param); 47 | 48 | if (result) 49 | return result; 50 | 51 | mpu_set_bypass(1); // Place all slaves (including compass) on primary bus 52 | 53 | setSensors(INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_XYZ_COMPASS); 54 | 55 | _gSense = getGyroSens(); 56 | _aSense = getAccelSens(); 57 | 58 | return result; 59 | } 60 | 61 | inv_error_t MPU9250_DMP::enableInterrupt(unsigned char enable) 62 | { 63 | return set_int_enable(enable); 64 | } 65 | 66 | inv_error_t MPU9250_DMP::setIntLevel(unsigned char active_low) 67 | { 68 | return mpu_set_int_level(active_low); 69 | } 70 | 71 | inv_error_t MPU9250_DMP::setIntLatched(unsigned char enable) 72 | { 73 | return mpu_set_int_latched(enable); 74 | } 75 | 76 | short MPU9250_DMP::getIntStatus(void) 77 | { 78 | short status; 79 | if (mpu_get_int_status(&status) == INV_SUCCESS) 80 | { 81 | return status; 82 | } 83 | return 0; 84 | } 85 | 86 | // Accelerometer Low-Power Mode. Rate options: 87 | // 1.25 (1), 2.5 (2), 5, 10, 20, 40, 88 | // 80, 160, 320, or 640 Hz 89 | // Disables compass and gyro 90 | inv_error_t MPU9250_DMP::lowPowerAccel(unsigned short rate) 91 | { 92 | return mpu_lp_accel_mode(rate); 93 | } 94 | 95 | inv_error_t MPU9250_DMP::setGyroFSR(unsigned short fsr) 96 | { 97 | inv_error_t err; 98 | err = mpu_set_gyro_fsr(fsr); 99 | if (err == INV_SUCCESS) 100 | { 101 | _gSense = getGyroSens(); 102 | } 103 | return err; 104 | } 105 | 106 | inv_error_t MPU9250_DMP::setAccelFSR(unsigned char fsr) 107 | { 108 | inv_error_t err; 109 | err = mpu_set_accel_fsr(fsr); 110 | if (err == INV_SUCCESS) 111 | { 112 | _aSense = getAccelSens(); 113 | } 114 | return err; 115 | } 116 | 117 | unsigned short MPU9250_DMP::getGyroFSR(void) 118 | { 119 | unsigned short tmp; 120 | if (mpu_get_gyro_fsr(&tmp) == INV_SUCCESS) 121 | { 122 | return tmp; 123 | } 124 | return 0; 125 | } 126 | 127 | unsigned char MPU9250_DMP::getAccelFSR(void) 128 | { 129 | unsigned char tmp; 130 | if (mpu_get_accel_fsr(&tmp) == INV_SUCCESS) 131 | { 132 | return tmp; 133 | } 134 | return 0; 135 | } 136 | 137 | unsigned short MPU9250_DMP::getMagFSR(void) 138 | { 139 | unsigned short tmp; 140 | if (mpu_get_compass_fsr(&tmp) == INV_SUCCESS) 141 | { 142 | return tmp; 143 | } 144 | return 0; 145 | } 146 | 147 | inv_error_t MPU9250_DMP::setLPF(unsigned short lpf) 148 | { 149 | return mpu_set_lpf(lpf); 150 | } 151 | 152 | unsigned short MPU9250_DMP::getLPF(void) 153 | { 154 | unsigned short tmp; 155 | if (mpu_get_lpf(&tmp) == INV_SUCCESS) 156 | { 157 | return tmp; 158 | } 159 | return 0; 160 | } 161 | 162 | inv_error_t MPU9250_DMP::setSampleRate(unsigned short rate) 163 | { 164 | return mpu_set_sample_rate(rate); 165 | } 166 | 167 | unsigned short MPU9250_DMP::getSampleRate(void) 168 | { 169 | unsigned short tmp; 170 | if (mpu_get_sample_rate(&tmp) == INV_SUCCESS) 171 | { 172 | return tmp; 173 | } 174 | return 0; 175 | } 176 | 177 | inv_error_t MPU9250_DMP::setCompassSampleRate(unsigned short rate) 178 | { 179 | return mpu_set_compass_sample_rate(rate); 180 | } 181 | 182 | unsigned short MPU9250_DMP::getCompassSampleRate(void) 183 | { 184 | unsigned short tmp; 185 | if (mpu_get_compass_sample_rate(&tmp) == INV_SUCCESS) 186 | { 187 | return tmp; 188 | } 189 | 190 | return 0; 191 | } 192 | 193 | float MPU9250_DMP::getGyroSens(void) 194 | { 195 | float sens; 196 | if (mpu_get_gyro_sens(&sens) == INV_SUCCESS) 197 | { 198 | return sens; 199 | } 200 | return 0; 201 | } 202 | 203 | unsigned short MPU9250_DMP::getAccelSens(void) 204 | { 205 | unsigned short sens; 206 | if (mpu_get_accel_sens(&sens) == INV_SUCCESS) 207 | { 208 | return sens; 209 | } 210 | return 0; 211 | } 212 | 213 | float MPU9250_DMP::getMagSens(void) 214 | { 215 | return 0.15; // Static, 4915/32760 216 | } 217 | 218 | unsigned char MPU9250_DMP::getFifoConfig(void) 219 | { 220 | unsigned char sensors; 221 | if (mpu_get_fifo_config(&sensors) == INV_SUCCESS) 222 | { 223 | return sensors; 224 | } 225 | return 0; 226 | } 227 | 228 | inv_error_t MPU9250_DMP::configureFifo(unsigned char sensors) 229 | { 230 | return mpu_configure_fifo(sensors); 231 | } 232 | 233 | inv_error_t MPU9250_DMP::resetFifo(void) 234 | { 235 | return mpu_reset_fifo(); 236 | } 237 | 238 | unsigned short MPU9250_DMP::fifoAvailable(void) 239 | { 240 | unsigned char fifoH, fifoL; 241 | 242 | if (mpu_read_reg(MPU9250_FIFO_COUNTH, &fifoH) != INV_SUCCESS) 243 | return 0; 244 | if (mpu_read_reg(MPU9250_FIFO_COUNTL, &fifoL) != INV_SUCCESS) 245 | return 0; 246 | 247 | return (fifoH << 8 ) | fifoL; 248 | } 249 | 250 | inv_error_t MPU9250_DMP::updateFifo(void) 251 | { 252 | short gyro[3], accel[3]; 253 | unsigned long timestamp; 254 | unsigned char sensors, more; 255 | 256 | if (mpu_read_fifo(gyro, accel, ×tamp, &sensors, &more) != INV_SUCCESS) 257 | return INV_ERROR; 258 | 259 | if (sensors & INV_XYZ_ACCEL) 260 | { 261 | ax = accel[X_AXIS]; 262 | ay = accel[Y_AXIS]; 263 | az = accel[Z_AXIS]; 264 | } 265 | if (sensors & INV_X_GYRO) 266 | gx = gyro[X_AXIS]; 267 | if (sensors & INV_Y_GYRO) 268 | gy = gyro[Y_AXIS]; 269 | if (sensors & INV_Z_GYRO) 270 | gz = gyro[Z_AXIS]; 271 | 272 | time = timestamp; 273 | 274 | return INV_SUCCESS; 275 | } 276 | 277 | inv_error_t MPU9250_DMP::setSensors(unsigned char sensors) 278 | { 279 | return mpu_set_sensors(sensors); 280 | } 281 | 282 | bool MPU9250_DMP::dataReady() 283 | { 284 | unsigned char intStatusReg; 285 | 286 | if (mpu_read_reg(MPU9250_INT_STATUS, &intStatusReg) == INV_SUCCESS) 287 | { 288 | return (intStatusReg & (1< MAX_DMP_SAMPLE_RATE) rate = MAX_DMP_SAMPLE_RATE; 413 | return dmp_set_fifo_rate(rate); 414 | } 415 | 416 | inv_error_t MPU9250_DMP::dmpUpdateFifo(void) 417 | { 418 | short gyro[3]; 419 | short accel[3]; 420 | long quat[4]; 421 | unsigned long timestamp; 422 | short sensors; 423 | unsigned char more; 424 | 425 | if (dmp_read_fifo(gyro, accel, quat, ×tamp, &sensors, &more) 426 | != INV_SUCCESS) 427 | { 428 | return INV_ERROR; 429 | } 430 | 431 | if (sensors & INV_XYZ_ACCEL) 432 | { 433 | ax = accel[X_AXIS]; 434 | ay = accel[Y_AXIS]; 435 | az = accel[Z_AXIS]; 436 | } 437 | if (sensors & INV_X_GYRO) 438 | gx = gyro[X_AXIS]; 439 | if (sensors & INV_Y_GYRO) 440 | gy = gyro[Y_AXIS]; 441 | if (sensors & INV_Z_GYRO) 442 | gz = gyro[Z_AXIS]; 443 | if (sensors & INV_WXYZ_QUAT) 444 | { 445 | qw = quat[0]; 446 | qx = quat[1]; 447 | qy = quat[2]; 448 | qz = quat[3]; 449 | } 450 | 451 | time = timestamp; 452 | 453 | return INV_SUCCESS; 454 | } 455 | 456 | inv_error_t MPU9250_DMP::dmpEnableFeatures(unsigned short mask) 457 | { 458 | unsigned short enMask = 0; 459 | enMask |= mask; 460 | // Combat known issue where fifo sample rate is incorrect 461 | // unless tap is enabled in the DMP. 462 | enMask |= DMP_FEATURE_TAP; 463 | return dmp_enable_feature(enMask); 464 | } 465 | 466 | unsigned short MPU9250_DMP::dmpGetEnabledFeatures(void) 467 | { 468 | unsigned short mask; 469 | if (dmp_get_enabled_features(&mask) == INV_SUCCESS) 470 | return mask; 471 | return 0; 472 | } 473 | 474 | inv_error_t MPU9250_DMP::dmpSetTap( 475 | unsigned short xThresh, unsigned short yThresh, unsigned short zThresh, 476 | unsigned char taps, unsigned short tapTime, unsigned short tapMulti) 477 | { 478 | unsigned char axes = 0; 479 | if (xThresh > 0) 480 | { 481 | axes |= TAP_X; 482 | xThresh = constrain(xThresh, 1, 1600); 483 | if (dmp_set_tap_thresh(1< 0) 487 | { 488 | axes |= TAP_Y; 489 | yThresh = constrain(yThresh, 1, 1600); 490 | if (dmp_set_tap_thresh(1< 0) 494 | { 495 | axes |= TAP_Z; 496 | zThresh = constrain(zThresh, 1, 1600); 497 | if (dmp_set_tap_thresh(1<> q) + ((number & mask) / (float) (2<<(q-1))); 621 | } 622 | 623 | void MPU9250_DMP::computeEulerAngles(bool degrees) 624 | { 625 | float dqw = qToFloat(qw, 30); 626 | float dqx = qToFloat(qx, 30); 627 | float dqy = qToFloat(qy, 30); 628 | float dqz = qToFloat(qz, 30); 629 | 630 | float ysqr = dqy * dqy; 631 | float t0 = -2.0f * (ysqr + dqz * dqz) + 1.0f; 632 | float t1 = +2.0f * (dqx * dqy - dqw * dqz); 633 | float t2 = -2.0f * (dqx * dqz + dqw * dqy); 634 | float t3 = +2.0f * (dqy * dqz - dqw * dqx); 635 | float t4 = -2.0f * (dqx * dqx + ysqr) + 1.0f; 636 | 637 | // Keep t2 within range of asin (-1, 1) 638 | t2 = t2 > 1.0f ? 1.0f : t2; 639 | t2 = t2 < -1.0f ? -1.0f : t2; 640 | 641 | pitch = asin(t2) * 2; 642 | roll = atan2(t3, t4); 643 | yaw = atan2(t1, t0); 644 | 645 | if (degrees) 646 | { 647 | pitch *= (180.0 / PI); 648 | roll *= (180.0 / PI); 649 | yaw *= (180.0 / PI); 650 | if (pitch < 0) pitch = 360.0 + pitch; 651 | if (roll < 0) roll = 360.0 + roll; 652 | if (yaw < 0) yaw = 360.0 + yaw; 653 | } 654 | } 655 | 656 | float MPU9250_DMP::computeCompassHeading(void) 657 | { 658 | if (my == 0) 659 | heading = (mx < 0) ? PI : 0; 660 | else 661 | heading = atan2(mx, my); 662 | 663 | if (heading > PI) heading -= (2 * PI); 664 | else if (heading < -PI) heading += (2 * PI); 665 | else if (heading < 0) heading += 2 * PI; 666 | 667 | heading*= 180.0 / PI; 668 | 669 | return heading; 670 | } 671 | 672 | unsigned short MPU9250_DMP::orientation_row_2_scale(const signed char *row) 673 | { 674 | unsigned short b; 675 | 676 | if (row[0] > 0) 677 | b = 0; 678 | else if (row[0] < 0) 679 | b = 4; 680 | else if (row[1] > 0) 681 | b = 1; 682 | else if (row[1] < 0) 683 | b = 5; 684 | else if (row[2] > 0) 685 | b = 2; 686 | else if (row[2] < 0) 687 | b = 6; 688 | else 689 | b = 7; // error 690 | return b; 691 | } 692 | 693 | static void tap_cb(unsigned char direction, unsigned char count) 694 | { 695 | _tap_available = true; 696 | tap_count = count; 697 | tap_direction = direction; 698 | } 699 | 700 | static void orient_cb(unsigned char orient) 701 | { 702 | mpu9250_orientation = orient; 703 | } -------------------------------------------------------------------------------- /src/SparkFunMPU9250-DMP.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | SparkFunMPU9250-DMP.h - MPU-9250 Digital Motion Processor Arduino Library 3 | Jim Lindblom @ SparkFun Electronics 4 | original creation date: November 23, 2016 5 | https://github.com/sparkfun/SparkFun_MPU9250_DMP_Arduino_Library 6 | 7 | This library implements motion processing functions of Invensense's MPU-9250. 8 | It is based on their Emedded MotionDriver 6.12 library. 9 | https://www.invensense.com/developers/software-downloads/ 10 | 11 | Development environment specifics: 12 | Arduino IDE 1.6.12 13 | SparkFun 9DoF Razor IMU M0 14 | 15 | Supported Platforms: 16 | - ATSAMD21 (Arduino Zero, SparkFun SAMD21 Breakouts) 17 | ******************************************************************************/ 18 | #ifndef _SPARKFUN_MPU9250_DMP_H_ 19 | #define _SPARKFUN_MPU9250_DMP_H_ 20 | 21 | #include 22 | #include 23 | 24 | // Optimally, these defines would be passed as compiler options, but Arduino 25 | // doesn't give us a great way to do that. 26 | #define MPU9250 27 | #define AK8963_SECONDARY 28 | #define COMPASS_ENABLED 29 | 30 | // Include the Invensense MPU9250 driver and DMP keys: 31 | extern "C" { 32 | #include "util/inv_mpu.h" 33 | #include "util/inv_mpu_dmp_motion_driver.h" 34 | } 35 | 36 | typedef int inv_error_t; 37 | #define INV_SUCCESS 0 38 | #define INV_ERROR 0x20 39 | 40 | enum t_axisOrder { 41 | X_AXIS, // 0 42 | Y_AXIS, // 1 43 | Z_AXIS // 2 44 | }; 45 | 46 | // Define's passed to update(), to request a specific sensor (or multiple): 47 | #define UPDATE_ACCEL (1<<1) 48 | #define UPDATE_GYRO (1<<2) 49 | #define UPDATE_COMPASS (1<<3) 50 | #define UPDATE_TEMP (1<<4) 51 | 52 | #define INT_ACTIVE_HIGH 0 53 | #define INT_ACTIVE_LOW 1 54 | #define INT_LATCHED 1 55 | #define INT_50US_PULSE 0 56 | 57 | #define MAX_DMP_SAMPLE_RATE 200 // Maximum sample rate for the DMP FIFO (200Hz) 58 | #define FIFO_BUFFER_SIZE 512 // Max FIFO buffer size 59 | 60 | const signed char defaultOrientation[9] = { 61 | 1, 0, 0, 62 | 0, 1, 0, 63 | 0, 0, 1 64 | }; 65 | #define ORIENT_PORTRAIT 0 66 | #define ORIENT_LANDSCAPE 1 67 | #define ORIENT_REVERSE_PORTRAIT 2 68 | #define ORIENT_REVERSE_LANDSCAPE 3 69 | 70 | class MPU9250_DMP 71 | { 72 | public: 73 | int ax, ay, az; 74 | int gx, gy, gz; 75 | int mx, my, mz; 76 | long qw, qx, qy, qz; 77 | long temperature; 78 | unsigned long time; 79 | float pitch, roll, yaw; 80 | float heading; 81 | 82 | MPU9250_DMP(); 83 | 84 | // begin(void) -- Verifies communication with the MPU-9250 and the AK8963, 85 | // and initializes them to the default state: 86 | // All sensors enabled 87 | // Gyro FSR: +/- 2000 dps 88 | // Accel FSR: +/- 2g 89 | // LPF: 42 Hz 90 | // FIFO: 50 Hz, disabled 91 | // Output: INV_SUCCESS (0) on success, otherwise error 92 | inv_error_t begin(void); 93 | 94 | // setSensors(unsigned char) -- Turn on or off MPU-9250 sensors. Any of the 95 | // following defines can be combined: INV_XYZ_GYRO, INV_XYZ_ACCEL, 96 | // INV_XYZ_COMPASS, INV_X_GYRO, INV_Y_GYRO, or INV_Z_GYRO 97 | // Input: Combination of enabled sensors. Unless specified a sensor will be 98 | // disabled. 99 | // Output: INV_SUCCESS (0) on success, otherwise error 100 | inv_error_t setSensors(unsigned char sensors); 101 | 102 | // setGyroFSR(unsigned short) -- Sets the full-scale range of the gyroscope 103 | // Input: Gyro DPS - 250, 500, 1000, or 2000 104 | // Output: INV_SUCCESS (0) on success, otherwise error 105 | inv_error_t setGyroFSR(unsigned short fsr); 106 | // getGyroFSR -- Returns the current gyroscope FSR 107 | // Output: Current Gyro DPS - 250, 500, 1000, or 2000 108 | unsigned short getGyroFSR(void); 109 | // getGyroSens -- Returns current gyroscope sensitivity. The FSR divided by 110 | // the resolution of the sensor (signed 16-bit). 111 | // Output: Currently set gyroscope sensitivity (e.g. 131, 65.5, 32.8, 16.4) 112 | float getGyroSens(void); 113 | 114 | // setAccelFSR(unsigned short) -- Sets the FSR of the accelerometer 115 | // 116 | // Input: Accel g range - 2, 4, 8, or 16 117 | // Output: INV_SUCCESS (0) on success, otherwise error 118 | inv_error_t setAccelFSR(unsigned char fsr); 119 | // getAccelFSR -- Returns the current accelerometer FSR 120 | // Output: Current Accel g - 2, 4, 8, or 16 121 | unsigned char getAccelFSR(void); 122 | // getAccelSens -- Returns current accelerometer sensitivity. The FSR 123 | // divided by the resolution of the sensor (signed 16-bit). 124 | // Output: Currently set accel sensitivity (e.g. 16384, 8192, 4096, 2048) 125 | unsigned short getAccelSens(void); 126 | 127 | // getMagFSR -- Returns the current magnetometer FSR 128 | // Output: Current mag uT range - +/-1450 uT 129 | unsigned short getMagFSR(void); 130 | // getMagSens -- Returns current magnetometer sensitivity. The FSR 131 | // divided by the resolution of the sensor (signed 16-bit). 132 | // Output: Currently set mag sensitivity (e.g. 0.15) 133 | float getMagSens(void); 134 | 135 | // setLPF -- Sets the digital low-pass filter of the accel and gyro. 136 | // Can be any of the following: 188, 98, 42, 20, 10, 5 (value in Hz) 137 | // Input: 188, 98, 42, 20, 10, or 5 (defaults to 5 if incorrectly set) 138 | // Output: INV_SUCCESS (0) on success, otherwise error 139 | inv_error_t setLPF(unsigned short lpf); 140 | // getLPF -- Returns the set value of the LPF. 141 | // 142 | // Output: 5, 10, 20, 42, 98, or 188 if set. 0 if the LPF is disabled. 143 | unsigned short getLPF(void); 144 | 145 | // setSampleRate -- Set the gyroscope and accelerometer sample rate to a 146 | // value between 4Hz and 1000Hz (1kHz). 147 | // The library will make an attempt to get as close as possible to the 148 | // requested sample rate. 149 | // Input: Value between 4 and 1000, indicating the desired sample rate 150 | // Output: INV_SUCCESS (0) on success, otherwise error 151 | inv_error_t setSampleRate(unsigned short rate); 152 | // getSampleRate -- Get the currently set sample rate. 153 | // May differ slightly from what was set in setSampleRate. 154 | // Output: set sample rate of the accel/gyro. A value between 4-1000. 155 | unsigned short getSampleRate(void); 156 | 157 | // setCompassSampleRate -- Set the magnetometer sample rate to a value 158 | // between 1Hz and 100 Hz. 159 | // The library will make an attempt to get as close as possible to the 160 | // requested sample rate. 161 | // Input: Value between 1 and 100, indicating the desired sample rate 162 | // Output: INV_SUCCESS (0) on success, otherwise error 163 | inv_error_t setCompassSampleRate(unsigned short rate); 164 | // getCompassSampleRate -- Get the currently set magnetometer sample rate. 165 | // May differ slightly from what was set in setCompassSampleRate. 166 | // 167 | // Output: set sample rate of the magnetometer. A value between 1-100 168 | unsigned short getCompassSampleRate(void); 169 | 170 | // dataReady -- checks to see if new accel/gyro data is available. 171 | // (New magnetometer data cannot be checked, as the library runs that sensor 172 | // in single-conversion mode.) 173 | // Output: true if new accel/gyro data is available 174 | bool dataReady(); 175 | 176 | // update -- Reads latest data from the MPU-9250's data registers. 177 | // Sensors to be updated can be set using the [sensors] parameter. 178 | // [sensors] can be any combination of UPDATE_ACCEL, UPDATE_GYRO, 179 | // UPDATE_COMPASS, and UPDATE_TEMP. 180 | // Output: INV_SUCCESS (0) on success, otherwise error 181 | // Note: after a successful update the public sensor variables 182 | // (e.g. ax, ay, az, gx, gy, gz) will be updated with new data 183 | inv_error_t update(unsigned char sensors = 184 | UPDATE_ACCEL | UPDATE_GYRO | UPDATE_COMPASS); 185 | 186 | // updateAccel, updateGyro, updateCompass, and updateTemperature are 187 | // called by the update() public method. They read from their respective 188 | // sensor and update the class variable (e.g. ax, ay, az) 189 | // Output: INV_SUCCESS (0) on success, otherwise error 190 | inv_error_t updateAccel(void); 191 | inv_error_t updateGyro(void); 192 | inv_error_t updateCompass(void); 193 | inv_error_t updateTemperature(void); 194 | 195 | // configureFifo(unsigned char) -- Initialize the FIFO, set it to read from 196 | // a select set of sensors. 197 | // Any of the following defines can be combined for the [sensors] parameter: 198 | // INV_XYZ_GYRO, INV_XYZ_ACCEL, INV_X_GYRO, INV_Y_GYRO, or INV_Z_GYRO 199 | // Input: Combination of sensors to be read into FIFO 200 | // Output: INV_SUCCESS (0) on success, otherwise error 201 | inv_error_t configureFifo(unsigned char sensors); 202 | // getFifoConfig -- Returns the sensors configured to be read into the FIFO 203 | // Output: combination of INV_XYZ_GYRO, INV_XYZ_ACCEL, INV_Y_GYRO, 204 | // INV_X_GYRO, or INV_Z_GYRO 205 | unsigned char getFifoConfig(void); 206 | // fifoAvailable -- Returns the number of bytes currently filled in the FIFO 207 | // Outputs: Number of bytes filled in the FIFO (up to 512) 208 | unsigned short fifoAvailable(void); 209 | // updateFifo -- Reads from the top of the FIFO, and stores the new data 210 | // in ax, ay, az, gx, gy, or gz (depending on how the FIFO is configured). 211 | // Output: INV_SUCCESS (0) on success, otherwise error 212 | inv_error_t updateFifo(void); 213 | // resetFifo -- Resets the FIFO's read/write pointers 214 | // Output: INV_SUCCESS (0) on success, otherwise error 215 | inv_error_t resetFifo(void); 216 | 217 | // enableInterrupt -- Configure the MPU-9250's interrupt output to indicate 218 | // when new data is ready. 219 | // Input: 0 to disable, >=1 to enable 220 | // Output: INV_SUCCESS (0) on success, otherwise error 221 | inv_error_t enableInterrupt(unsigned char enable = 1); 222 | // setIntLevel -- Configure the MPU-9250's interrupt to be either active- 223 | // high or active-low. 224 | // Input: 0 for active-high, 1 for active-low 225 | // Output: INV_SUCCESS (0) on success, otherwise error 226 | inv_error_t setIntLevel(unsigned char active_low); 227 | // setIntLatched -- Configure the MPU-9250's interrupt to latch or operate 228 | // as a 50us pulse. 229 | // Input: 0 for 230 | // Output: INV_SUCCESS (0) on success, otherwise error 231 | inv_error_t setIntLatched(unsigned char enable); 232 | // getIntStatus -- Reads the MPU-9250's INT_STATUS register, which can 233 | // indicate what (if anything) caused an interrupt (e.g. FIFO overflow or 234 | // or data read). 235 | // Output: contents of the INT_STATUS register 236 | short getIntStatus(void); 237 | 238 | // dmpBegin -- Initialize the DMP, enable one or more features, and set the FIFO's sample rate 239 | // features can be any one of 240 | // DMP_FEATURE_TAP -- Tap detection 241 | // DMP_FEATURE_ANDROID_ORIENT -- Orientation (portrait/landscape) detection 242 | // DMP_FEATURE_LP_QUAT -- Accelerometer, low-power quaternion calculation 243 | // DMP_FEATURE_PEDOMETER -- Pedometer (always enabled) 244 | // DMP_FEATURE_6X_LP_QUAT -- 6-axis (accel/gyro) quaternion calculation 245 | // DMP_FEATURE_GYRO_CAL -- Gyroscope calibration (0's out after 8 seconds of no motion) 246 | // DMP_FEATURE_SEND_RAW_ACCEL -- Send raw accelerometer values to FIFO 247 | // DMP_FEATURE_SEND_RAW_GYRO -- Send raw gyroscope values to FIFO 248 | // DMP_FEATURE_SEND_CAL_GYRO -- Send calibrated gyroscop values to FIFO 249 | // fifoRate can be anywhere between 4 and 200Hz. 250 | // Input: OR'd list of features and requested FIFO sampling rate 251 | // Output: INV_SUCCESS (0) on success, otherwise error 252 | inv_error_t dmpBegin(unsigned short features = 0, unsigned short fifoRate = MAX_DMP_SAMPLE_RATE); 253 | 254 | // dmpLoad -- Loads the DMP with 3062-byte image memory. Must be called to begin DMP. 255 | // This function is called by the dmpBegin function. 256 | // Output: INV_SUCCESS (0) on success, otherwise error 257 | inv_error_t dmpLoad(void); 258 | 259 | // dmpGetFifoRate -- Returns the sample rate of the FIFO 260 | // Output: Set sample rate, in Hz, of the FIFO 261 | unsigned short dmpGetFifoRate(void); 262 | // dmpSetFiFoRate -- Sets the rate of the FIFO. 263 | // Input: Requested sample rate in Hz (range: 4-200) 264 | // Output: INV_SUCCESS (0) on success, otherwise error 265 | inv_error_t dmpSetFifoRate(unsigned short rate); 266 | 267 | // dmpUpdateFifo -- Reads from the top of the FIFO and fills accelerometer, gyroscope, 268 | // quaternion, and time public variables (depending on how the DMP is configured). 269 | // Should be called whenever an MPU interrupt is detected 270 | // Output: INV_SUCCESS (0) on success, otherwise error 271 | inv_error_t dmpUpdateFifo(void); 272 | 273 | // dmpEnableFeatures -- Enable one, or multiple DMP features. 274 | // Input: An OR'd list of features (see dmpBegin) 275 | // Output: INV_SUCCESS (0) on success, otherwise error 276 | inv_error_t dmpEnableFeatures(unsigned short mask); 277 | // dmpGetEnabledFeatures -- Returns the OR'd list of enabled DMP features 278 | // 279 | // Output: OR'd list of DMP feature's (see dmpBegin for list) 280 | unsigned short dmpGetEnabledFeatures(void); 281 | 282 | // dmpSetTap -- Enable tap detection and configure threshold, tap time, and minimum tap count. 283 | // Inputs: x/y/zThresh - accelerometer threshold on each axis. Range: 0 to 1600. 0 disables tap 284 | // detection on that axis. Units are mg/ms. 285 | // taps - minimum number of taps to create a tap event (Range: 1-4) 286 | // tapTime - Minimum number of milliseconds between separate taps 287 | // tapMulti - Maximum number of milliseconds combined taps 288 | // Output: INV_SUCCESS (0) on success, otherwise error 289 | inv_error_t dmpSetTap(unsigned short xThresh = 250, 290 | unsigned short yThresh = 250, 291 | unsigned short zThresh = 250, 292 | unsigned char taps = 1, 293 | unsigned short tapTime = 100, 294 | unsigned short tapMulti = 500); 295 | // tapAvailable -- Returns true if a new tap is available 296 | // Output: True if new tap data is available. Cleared on getTapDir or getTapCount. 297 | bool tapAvailable(void); 298 | // getTapDir -- Returns the tap direction. 299 | // Output: One of the following: TAP_X_UP, TAP_X_DOWN, TAP_Y_UP, TAP_Y_DOWN, TAP_Z_UP, 300 | // or TAP_Z_DOWN 301 | unsigned char getTapDir(void); 302 | // getTapCount -- Returns the number of taps in the sensed direction 303 | // Output: Value between 1-8 indicating successive number of taps sensed. 304 | unsigned char getTapCount(void); 305 | 306 | // dmpSetOrientation -- Set orientation matrix, used for orientation sensing. 307 | // Use defaultOrientation matrix as an example input. 308 | // Input: Gyro and accel orientation in body frame (9-byte array) 309 | // Output: INV_SUCCESS (0) on success, otherwise error 310 | inv_error_t dmpSetOrientation(const signed char * orientationMatrix = defaultOrientation); 311 | // dmpGetOrientation -- Get the orientation, if any. 312 | // Output: If an orientation is detected, one of ORIENT_LANDSCAPE, ORIENT_PORTRAIT, 313 | // ORIENT_REVERSE_LANDSCAPE, or ORIENT_REVERSE_PORTRAIT. 314 | unsigned char dmpGetOrientation(void); 315 | 316 | // dmpEnable3Quat -- Enable 3-axis quaternion calculation 317 | // Output: INV_SUCCESS (0) on success, otherwise error 318 | inv_error_t dmpEnable3Quat(void); 319 | 320 | // dmpEnable6Quat -- Enable 6-axis quaternion calculation 321 | // Output: INV_SUCCESS (0) on success, otherwise error 322 | inv_error_t dmpEnable6Quat(void); 323 | 324 | // dmpGetPedometerSteps -- Get number of steps in pedometer register 325 | // Output: Number of steps sensed 326 | unsigned long dmpGetPedometerSteps(void); 327 | // dmpSetPedometerSteps -- Set number of steps to a value 328 | // Input: Desired number of steps to begin incrementing from 329 | // Output: INV_SUCCESS (0) on success, otherwise error 330 | inv_error_t dmpSetPedometerSteps(unsigned long steps); 331 | // dmpGetPedometerTime -- Get number of milliseconds ellapsed over stepping 332 | // Output: Number of milliseconds where steps were detected 333 | unsigned long dmpGetPedometerTime(void); 334 | // dmpSetPedometerTime -- Set number time to begin incrementing step time counter from 335 | // Input: Desired number of milliseconds 336 | // Output: INV_SUCCESS (0) on success, otherwise error 337 | inv_error_t dmpSetPedometerTime(unsigned long time); 338 | 339 | // dmpSetInterruptMode -- 340 | // Output: INV_SUCCESS (0) on success, otherwise error 341 | inv_error_t dmpSetInterruptMode(unsigned char mode); 342 | // dmpSetGyroBias -- 343 | // Output: INV_SUCCESS (0) on success, otherwise error 344 | inv_error_t dmpSetGyroBias(long * bias); 345 | // dmpSetAccelBias -- 346 | // Output: INV_SUCCESS (0) on success, otherwise error 347 | inv_error_t dmpSetAccelBias(long * bias); 348 | 349 | // lowPowerAccel -- 350 | // Output: INV_SUCCESS (0) on success, otherwise error 351 | inv_error_t lowPowerAccel(unsigned short rate); 352 | 353 | // calcAccel -- Convert 16-bit signed acceleration value to g's 354 | float calcAccel(int axis); 355 | // calcGyro -- Convert 16-bit signed gyroscope value to degree's per second 356 | float calcGyro(int axis); 357 | // calcMag -- Convert 16-bit signed magnetometer value to microtesla (uT) 358 | float calcMag(int axis); 359 | // calcQuat -- Convert Q30-format quaternion to a vector between +/- 1 360 | float calcQuat(long axis); 361 | 362 | // computeEulerAngles -- Compute euler angles based on most recently read qw, qx, qy, and qz 363 | // Input: boolean indicating whether angle results are presented in degrees or radians 364 | // Output: class variables roll, pitch, and yaw will be updated on exit. 365 | void computeEulerAngles(bool degrees = true); 366 | 367 | // computeCompassHeading -- Compute heading based on most recently read mx, my, and mz values 368 | // Output: class variable heading will be updated on exit 369 | float computeCompassHeading(void); 370 | 371 | // selfTest -- Run gyro and accel self-test. 372 | // Output: Returns bit mask, 1 indicates success. A 0x7 is success on all sensors. 373 | // Bit pos 0: gyro 374 | // Bit pos 1: accel 375 | // Bit pos 2: mag 376 | int selfTest(unsigned char debug = 0); 377 | 378 | private: 379 | unsigned short _aSense; 380 | float _gSense, _mSense; 381 | 382 | // Convert a QN-format number to a float 383 | float qToFloat(long number, unsigned char q); 384 | unsigned short orientation_row_2_scale(const signed char *row); 385 | }; 386 | 387 | #endif // _SPARKFUN_MPU9250_DMP_H_ -------------------------------------------------------------------------------- /src/util/MPU9250_RegisterMap.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | MPU9250_RegisterMap.h - MPU-9250 Digital Motion Processor Arduino Library 3 | Jim Lindblom @ SparkFun Electronics 4 | original creation date: November 23, 2016 5 | https://github.com/sparkfun/SparkFun_MPU9250_DMP_Arduino_Library 6 | 7 | This library implements motion processing functions of Invensense's MPU-9250. 8 | It is based on their Emedded MotionDriver 6.12 library. 9 | https://www.invensense.com/developers/software-downloads/ 10 | 11 | Development environment specifics: 12 | Arduino IDE 1.6.12 13 | SparkFun 9DoF Razor IMU M0 14 | 15 | Supported Platforms: 16 | - ATSAMD21 (Arduino Zero, SparkFun SAMD21 Breakouts) 17 | ******************************************************************************/ 18 | #ifndef _MPU9250_REGISTER_MAP_H_ 19 | #define _MPU9250_REGISTER_MAP_H_ 20 | 21 | enum mpu9250_register { 22 | MPU9250_SELF_TEST_X_GYRO = 0x00, 23 | MPU9250_SELF_TEST_Y_GYRO = 0x01, 24 | MPU9250_SELF_TEST_Z_GYRO = 0x02, 25 | MPU9250_SELF_TEST_X_ACCEL = 0x0D, 26 | MPU9250_SELF_TEST_Y_ACCEL = 0x0E, 27 | MPU9250_SELF_TEST_Z_ACCEL = 0x0F, 28 | MPU9250_XG_OFFSET_H = 0x13, 29 | MPU9250_XG_OFFSET_L = 0x14, 30 | MPU9250_YG_OFFSET_H = 0x15, 31 | MPU9250_YG_OFFSET_L = 0x16, 32 | MPU9250_ZG_OFFSET_H = 0x17, 33 | MPU9250_ZG_OFFSET_L = 0x18, 34 | MPU9250_SMPLRT_DIV = 0x19, 35 | MPU9250_CONFIG = 0x1A, 36 | MPU9250_GYRO_CONFIG = 0x1B, 37 | MPU9250_ACCEL_CONFIG = 0x1C, 38 | MPU9250_ACCEL_CONFIG_2 = 0x1D, 39 | MPU9250_LP_ACCEL_ODR = 0x1E, 40 | MPU9250_WOM_THR = 0x1F, 41 | MPU9250_FIFO_EN = 0x23, 42 | MPU9250_I2C_MST_CTRL = 0x24, 43 | MPU9250_I2C_SLV0_ADDR = 0x25, 44 | MPU9250_I2C_SLV0_REG = 0x26, 45 | MPU9250_I2C_SLV0_CTRL = 0x27, 46 | MPU9250_I2C_SLV1_ADDR = 0x28, 47 | MPU9250_I2C_SLV1_REG = 0x29, 48 | MPU9250_I2C_SLV1_CTRL = 0x2A, 49 | MPU9250_I2C_SLV2_ADDR = 0x2B, 50 | MPU9250_I2C_SLV2_REG = 0x2C, 51 | MPU9250_I2C_SLV2_CTRL = 0x2D, 52 | MPU9250_I2C_SLV3_ADDR = 0x2E, 53 | MPU9250_I2C_SLV3_REG = 0x2F, 54 | MPU9250_I2C_SLV3_CTRL = 0x30, 55 | MPU9250_I2C_SLV4_ADDR = 0x31, 56 | MPU9250_I2C_SLV4_REG = 0x32, 57 | MPU9250_I2C_SLV4_DO = 0x33, 58 | MPU9250_I2C_SLV4_CTRL = 0x34, 59 | MPU9250_I2C_SLV4_DI = 0x35, 60 | MPU9250_I2C_MST_STATUS = 0x36, 61 | MPU9250_INT_PIN_CFG = 0x37, 62 | MPU9250_INT_ENABLE = 0x38, 63 | MPU9250_INT_STATUS = 0x3A, 64 | MPU9250_ACCEL_XOUT_H = 0x3B, 65 | MPU9250_ACCEL_XOUT_L = 0x3C, 66 | MPU9250_ACCEL_YOUT_H = 0x3D, 67 | MPU9250_ACCEL_YOUT_L = 0x3E, 68 | MPU9250_ACCEL_ZOUT_H = 0x3F, 69 | MPU9250_ACCEL_ZOUT_L = 0x40, 70 | MPU9250_TEMP_OUT_H = 0x41, 71 | MPU9250_TEMP_OUT_L = 0x42, 72 | MPU9250_GYRO_XOUT_H = 0x43, 73 | MPU9250_GYRO_XOUT_L = 0x44, 74 | MPU9250_GYRO_YOUT_H = 0x45, 75 | MPU9250_GYRO_YOUT_L = 0x46, 76 | MPU9250_GYRO_ZOUT_H = 0x47, 77 | MPU9250_GYRO_ZOUT_L = 0x48, 78 | MPU9250_EXT_SENS_DATA_00 = 0x49, 79 | MPU9250_EXT_SENS_DATA_01 = 0x4A, 80 | MPU9250_EXT_SENS_DATA_02 = 0x4B, 81 | MPU9250_EXT_SENS_DATA_03 = 0x4C, 82 | MPU9250_EXT_SENS_DATA_04 = 0x4D, 83 | MPU9250_EXT_SENS_DATA_05 = 0x4E, 84 | MPU9250_EXT_SENS_DATA_06 = 0x4F, 85 | MPU9250_EXT_SENS_DATA_07 = 0x50, 86 | MPU9250_EXT_SENS_DATA_08 = 0x51, 87 | MPU9250_EXT_SENS_DATA_09 = 0x52, 88 | MPU9250_EXT_SENS_DATA_10 = 0x53, 89 | MPU9250_EXT_SENS_DATA_11 = 0x54, 90 | MPU9250_EXT_SENS_DATA_12 = 0x55, 91 | MPU9250_EXT_SENS_DATA_13 = 0x56, 92 | MPU9250_EXT_SENS_DATA_14 = 0x57, 93 | MPU9250_EXT_SENS_DATA_15 = 0x58, 94 | MPU9250_EXT_SENS_DATA_16 = 0x59, 95 | MPU9250_EXT_SENS_DATA_17 = 0x5A, 96 | MPU9250_EXT_SENS_DATA_18 = 0x5B, 97 | MPU9250_EXT_SENS_DATA_19 = 0x5C, 98 | MPU9250_EXT_SENS_DATA_20 = 0x5D, 99 | MPU9250_EXT_SENS_DATA_21 = 0x5E, 100 | MPU9250_EXT_SENS_DATA_22 = 0x5F, 101 | MPU9250_EXT_SENS_DATA_23 = 0x60, 102 | MPU9250_I2C_SLV0_DO = 0x63, 103 | MPU9250_I2C_SLV1_DO = 0x64, 104 | MPU9250_I2C_SLV2_DO = 0x65, 105 | MPU9250_I2C_SLV3_DO = 0x66, 106 | MPU9250_I2C_MST_DELAY_CTRL =0x67, 107 | MPU9250_SIGNAL_PATH_RESET = 0x68, 108 | MPU9250_MOT_DETECT_CTRL = 0x69, 109 | MPU9250_USER_CTRL = 0x6A, 110 | MPU9250_PWR_MGMT_1 = 0x6B, 111 | MPU9250_PWR_MGMT_2 = 0x6C, 112 | MPU9250_FIFO_COUNTH = 0x72, 113 | MPU9250_FIFO_COUNTL = 0x73, 114 | MPU9250_FIFO_R_W = 0x74, 115 | MPU9250_WHO_AM_I = 0x75, 116 | MPU9250_XA_OFFSET_H = 0x77, 117 | MPU9250_XA_OFFSET_L = 0x78, 118 | MPU9250_YA_OFFSET_H = 0x7A, 119 | MPU9250_YA_OFFSET_L = 0x7B, 120 | MPU9250_ZA_OFFSET_H = 0x7D, 121 | MPU9250_ZA_OFFSET_L = 0x7E 122 | }; 123 | 124 | enum interrupt_status_bits { 125 | INT_STATUS_RAW_DATA_RDY_INT = 0, 126 | INT_STATUS_FSYNC_INT = 3, 127 | INT_STATUS_FIFO_OVERFLOW_INT = 4, 128 | INT_STATUS_WOM_INT = 6, 129 | }; 130 | 131 | enum gyro_config_bits { 132 | GYRO_CONFIG_FCHOICE_B = 0, 133 | GYRO_CONFIG_GYRO_FS_SEL = 3, 134 | GYRO_CONFIG_ZGYRO_CTEN = 5, 135 | GYRO_CONFIG_YGYRO_CTEN = 6, 136 | GYRO_CONFIG_XGYRO_CTEN = 7, 137 | }; 138 | #define MPU9250_GYRO_FS_SEL_MASK 0x3 139 | #define MPU9250_GYRO_FCHOICE_MASK 0x3 140 | 141 | enum accel_config_bit { 142 | ACCEL_CONFIG_ACCEL_FS_SEL = 3, 143 | ACCEL_CONFIG_AZ_ST_EN = 5, 144 | ACCEL_CONFIG_AY_ST_EN = 6, 145 | ACCEL_CONFIG_AX_ST_EN = 7, 146 | }; 147 | #define MPU9250_ACCEL_FS_SEL_MASK 0x3 148 | 149 | enum accel_config_2_bits { 150 | ACCEL_CONFIG_2_A_DLPFCFG = 0, 151 | ACCEL_CONFIG_2_ACCEL_FCHOICE_B = 3, 152 | }; 153 | 154 | enum pwr_mgmt_1_bits { 155 | PWR_MGMT_1_CLKSEL = 0, 156 | PWR_MGMT_1_PD_PTAT = 3, 157 | PWR_MGMT_1_GYRO_STANDBY = 4, 158 | PWR_MGMT_1_CYCLE = 5, 159 | PWR_MGMT_1_SLEEP = 6, 160 | PWR_MGMT_1_H_RESET = 7 161 | }; 162 | 163 | enum pwr_mgmt_2_bits { 164 | PWR_MGMT_2_DISABLE_ZG = 0, 165 | PWR_MGMT_2_DISABLE_YG = 1, 166 | PWR_MGMT_2_DISABLE_XG = 2, 167 | PWR_MGMT_2_DISABLE_ZA = 3, 168 | PWR_MGMT_2_DISABLE_YA = 4, 169 | PWR_MGMT_2_DISABLE_XA = 5, 170 | }; 171 | 172 | enum int_enable_bits { 173 | INT_ENABLE_RAW_RDY_EN = 0, 174 | INT_ENABLE_FSYNC_INT_EN = 3, 175 | INT_ENABLE_FIFO_OVERFLOW_EN = 4, 176 | INT_ENABLE_WOM_EN = 6, 177 | }; 178 | 179 | enum int_pin_cfg_bits { 180 | INT_PIN_CFG_BYPASS_EN = 1, 181 | INT_PIN_CFG_FSYNC_INT_MODE_EN = 2, 182 | INT_PIN_CFG_ACTL_FSYNC = 3, 183 | INT_PIN_CFG_INT_ANYRD_2CLEAR = 4, 184 | INT_PIN_CFG_LATCH_INT_EN = 5, 185 | INT_PIN_CFG_OPEN = 6, 186 | INT_PIN_CFG_ACTL = 7, 187 | }; 188 | #define INT_PIN_CFG_INT_MASK 0xF0 189 | 190 | #define MPU9250_WHO_AM_I_RESULT 0x71 191 | 192 | enum ak8963_register { 193 | AK8963_WIA = 0x0, 194 | AK8963_INFO = 0x1, 195 | AK8963_ST1 = 0x2, 196 | AK8963_HXL = 0x3, 197 | AK8963_HXH = 0x4, 198 | AK8963_HYL = 0x5, 199 | AK8963_HYH = 0x6, 200 | AK8963_HZL = 0x7, 201 | AK8963_HZH = 0x8, 202 | AK8963_ST2 = 0x9, 203 | AK8963_CNTL = 0xA, 204 | AK8963_RSV = 0xB, 205 | AK8963_ASTC = 0xC, 206 | AK8963_TS1 = 0xD, 207 | AK8963_TS2 = 0xE, 208 | AK8963_I2CDIS = 0xF, 209 | AK8963_ASAX = 0x10, 210 | AK8963_ASAY = 0x11, 211 | AK8963_ASAZ = 0x12, 212 | }; 213 | #define MAG_CTRL_OP_MODE_MASK 0xF 214 | 215 | #define AK8963_ST1_DRDY_BIT 0 216 | 217 | #define AK8963_WHO_AM_I_RESULT 0x48 218 | 219 | #endif // _MPU9250_REGISTER_MAP_H_ -------------------------------------------------------------------------------- /src/util/arduino_mpu9250_clk.c: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | arduino_mpu9250_clk.c - MPU-9250 Digital Motion Processor Arduino Library 3 | Jim Lindblom @ SparkFun Electronics 4 | original creation date: November 23, 2016 5 | https://github.com/sparkfun/SparkFun_MPU9250_DMP_Arduino_Library 6 | 7 | This library implements motion processing functions of Invensense's MPU-9250. 8 | It is based on their Emedded MotionDriver 6.12 library. 9 | https://www.invensense.com/developers/software-downloads/ 10 | 11 | Development environment specifics: 12 | Arduino IDE 1.6.12 13 | SparkFun 9DoF Razor IMU M0 14 | 15 | Supported Platforms: 16 | - ATSAMD21 (Arduino Zero, SparkFun SAMD21 Breakouts) 17 | ******************************************************************************/ 18 | #include "arduino_mpu9250_clk.h" 19 | #include 20 | 21 | int arduino_get_clock_ms(unsigned long *count) 22 | { 23 | *count = millis(); 24 | return 0; 25 | } 26 | 27 | int arduino_delay_ms(unsigned long num_ms) 28 | { 29 | delay(num_ms); 30 | return 0; 31 | } -------------------------------------------------------------------------------- /src/util/arduino_mpu9250_clk.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | arduino_mpu9250_clk.h - MPU-9250 Digital Motion Processor Arduino Library 3 | Jim Lindblom @ SparkFun Electronics 4 | original creation date: November 23, 2016 5 | https://github.com/sparkfun/SparkFun_MPU9250_DMP_Arduino_Library 6 | 7 | This library implements motion processing functions of Invensense's MPU-9250. 8 | It is based on their Emedded MotionDriver 6.12 library. 9 | https://www.invensense.com/developers/software-downloads/ 10 | 11 | Development environment specifics: 12 | Arduino IDE 1.6.12 13 | SparkFun 9DoF Razor IMU M0 14 | 15 | Supported Platforms: 16 | - ATSAMD21 (Arduino Zero, SparkFun SAMD21 Breakouts) 17 | ******************************************************************************/ 18 | #ifndef _ARDUINO_MPU9250_CLK_H_ 19 | #define _ARDUINO_MPU9250_CLK_H_ 20 | 21 | int arduino_get_clock_ms(unsigned long *count); 22 | int arduino_delay_ms(unsigned long num_ms); 23 | 24 | #endif // _ARDUINO_MPU9250_CLK_H_ -------------------------------------------------------------------------------- /src/util/arduino_mpu9250_i2c.cpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | arduino_mpu9250_i2c.cpp - MPU-9250 Digital Motion Processor Arduino Library 3 | Jim Lindblom @ SparkFun Electronics 4 | original creation date: November 23, 2016 5 | https://github.com/sparkfun/SparkFun_MPU9250_DMP_Arduino_Library 6 | 7 | This library implements motion processing functions of Invensense's MPU-9250. 8 | It is based on their Emedded MotionDriver 6.12 library. 9 | https://www.invensense.com/developers/software-downloads/ 10 | 11 | Development environment specifics: 12 | Arduino IDE 1.6.12 13 | SparkFun 9DoF Razor IMU M0 14 | 15 | Supported Platforms: 16 | - ATSAMD21 (Arduino Zero, SparkFun SAMD21 Breakouts) 17 | ******************************************************************************/ 18 | #include "arduino_mpu9250_i2c.h" 19 | #include 20 | #include 21 | 22 | int arduino_i2c_write(unsigned char slave_addr, unsigned char reg_addr, 23 | unsigned char length, unsigned char * data) 24 | { 25 | Wire.beginTransmission(slave_addr); 26 | Wire.write(reg_addr); 27 | for (unsigned char i = 0; i < length; i++) 28 | { 29 | Wire.write(data[i]); 30 | } 31 | Wire.endTransmission(true); 32 | 33 | return 0; 34 | } 35 | 36 | int arduino_i2c_read(unsigned char slave_addr, unsigned char reg_addr, 37 | unsigned char length, unsigned char * data) 38 | { 39 | Wire.beginTransmission(slave_addr); 40 | Wire.write(reg_addr); 41 | Wire.endTransmission(false); 42 | Wire.requestFrom(slave_addr, length); 43 | for (unsigned char i = 0; i < length; i++) 44 | { 45 | data[i] = Wire.read(); 46 | } 47 | 48 | return 0; 49 | } 50 | -------------------------------------------------------------------------------- /src/util/arduino_mpu9250_i2c.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | arduino_mpu9250_i2c.h - MPU-9250 Digital Motion Processor Arduino Library 3 | Jim Lindblom @ SparkFun Electronics 4 | original creation date: November 23, 2016 5 | https://github.com/sparkfun/SparkFun_MPU9250_DMP_Arduino_Library 6 | 7 | This library implements motion processing functions of Invensense's MPU-9250. 8 | It is based on their Emedded MotionDriver 6.12 library. 9 | https://www.invensense.com/developers/software-downloads/ 10 | 11 | Development environment specifics: 12 | Arduino IDE 1.6.12 13 | SparkFun 9DoF Razor IMU M0 14 | 15 | Supported Platforms: 16 | - ATSAMD21 (Arduino Zero, SparkFun SAMD21 Breakouts) 17 | ******************************************************************************/ 18 | #ifndef _ARDUINO_MPU9250_I2C_H_ 19 | #define _ARDUINO_MPU9250_I2C_H_ 20 | 21 | #if defined(__cplusplus) 22 | extern "C" { 23 | #endif 24 | 25 | int arduino_i2c_write(unsigned char slave_addr, unsigned char reg_addr, 26 | unsigned char length, unsigned char * data); 27 | int arduino_i2c_read(unsigned char slave_addr, unsigned char reg_addr, 28 | unsigned char length, unsigned char * data); 29 | 30 | #if defined(__cplusplus) 31 | } 32 | #endif 33 | 34 | #endif // _ARDUINO_MPU9250_I2C_H_ -------------------------------------------------------------------------------- /src/util/arduino_mpu9250_log.cpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | arduino_mpu9250_log.cpp - MPU-9250 Digital Motion Processor Arduino Library 3 | Jim Lindblom @ SparkFun Electronics 4 | original creation date: November 23, 2016 5 | https://github.com/sparkfun/SparkFun_MPU9250_DMP_Arduino_Library 6 | 7 | This library implements motion processing functions of Invensense's MPU-9250. 8 | It is based on their Emedded MotionDriver 6.12 library. 9 | https://www.invensense.com/developers/software-downloads/ 10 | 11 | Development environment specifics: 12 | Arduino IDE 1.6.12 13 | SparkFun 9DoF Razor IMU M0 14 | 15 | Supported Platforms: 16 | - ATSAMD21 (Arduino Zero, SparkFun SAMD21 Breakouts) 17 | ******************************************************************************/ 18 | #include "arduino_mpu9250_log.h" 19 | #include 20 | #include 21 | 22 | // Based on log_stm32.c from Invensense motion_driver_6.12 23 | 24 | #define BUF_SIZE (256) 25 | #define PACKET_LENGTH (23) 26 | 27 | #define PACKET_DEBUG (1) 28 | #define PACKET_QUAT (2) 29 | #define PACKET_DATA (3) 30 | 31 | void logString(char * string) 32 | { 33 | } 34 | 35 | int _MLPrintLog (int priority, const char* tag, const char* fmt, ...) 36 | { 37 | } 38 | 39 | void eMPL_send_quat(long *quat) 40 | { 41 | } 42 | 43 | void eMPL_send_data(unsigned char type, long *data) 44 | { 45 | } -------------------------------------------------------------------------------- /src/util/arduino_mpu9250_log.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | arduino_mpu9250_log.h - MPU-9250 Digital Motion Processor Arduino Library 3 | Jim Lindblom @ SparkFun Electronics 4 | original creation date: November 23, 2016 5 | https://github.com/sparkfun/SparkFun_MPU9250_DMP_Arduino_Library 6 | 7 | This library implements motion processing functions of Invensense's MPU-9250. 8 | It is based on their Emedded MotionDriver 6.12 library. 9 | https://www.invensense.com/developers/software-downloads/ 10 | 11 | Development environment specifics: 12 | Arduino IDE 1.6.12 13 | SparkFun 9DoF Razor IMU M0 14 | 15 | Supported Platforms: 16 | - ATSAMD21 (Arduino Zero, SparkFun SAMD21 Breakouts) 17 | ******************************************************************************/ 18 | #ifndef _ARDUINO_MPU9250_LOG_H_ 19 | #define _ARDUINO_MPU9250_LOG_H_ 20 | 21 | #define MPL_LOG_UNKNOWN (0) 22 | #define MPL_LOG_DEFAULT (1) 23 | #define MPL_LOG_VERBOSE (2) 24 | #define MPL_LOG_DEBUG (3) 25 | #define MPL_LOG_INFO (4) 26 | #define MPL_LOG_WARN (5) 27 | #define MPL_LOG_ERROR (6) 28 | #define MPL_LOG_SILENT (8) 29 | 30 | typedef enum { 31 | PACKET_DATA_ACCEL = 0, 32 | PACKET_DATA_GYRO, 33 | PACKET_DATA_COMPASS, 34 | PACKET_DATA_QUAT, 35 | PACKET_DATA_EULER, 36 | PACKET_DATA_ROT, 37 | PACKET_DATA_HEADING, 38 | PACKET_DATA_LINEAR_ACCEL, 39 | NUM_DATA_PACKETS 40 | } eMPL_packet_e; 41 | 42 | #if defined(__cplusplus) 43 | extern "C" { 44 | #endif 45 | 46 | #include 47 | 48 | void logString(char * string); 49 | int _MLPrintLog (int priority, const char* tag, const char* fmt, ...); 50 | void eMPL_send_quat(long *quat); 51 | void eMPL_send_data(unsigned char type, long *data); 52 | 53 | #if defined(__cplusplus) 54 | } 55 | #endif 56 | 57 | 58 | #endif // _ARDUINO_MPU9250_LOG_H_ -------------------------------------------------------------------------------- /src/util/dmpKey.h: -------------------------------------------------------------------------------- 1 | /* 2 | $License: 3 | Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. 4 | $ 5 | */ 6 | #ifndef DMPKEY_H__ 7 | #define DMPKEY_H__ 8 | 9 | #define KEY_CFG_25 (0) 10 | #define KEY_CFG_24 (KEY_CFG_25 + 1) 11 | #define KEY_CFG_26 (KEY_CFG_24 + 1) 12 | #define KEY_CFG_27 (KEY_CFG_26 + 1) 13 | #define KEY_CFG_21 (KEY_CFG_27 + 1) 14 | #define KEY_CFG_20 (KEY_CFG_21 + 1) 15 | #define KEY_CFG_TAP4 (KEY_CFG_20 + 1) 16 | #define KEY_CFG_TAP5 (KEY_CFG_TAP4 + 1) 17 | #define KEY_CFG_TAP6 (KEY_CFG_TAP5 + 1) 18 | #define KEY_CFG_TAP7 (KEY_CFG_TAP6 + 1) 19 | #define KEY_CFG_TAP0 (KEY_CFG_TAP7 + 1) 20 | #define KEY_CFG_TAP1 (KEY_CFG_TAP0 + 1) 21 | #define KEY_CFG_TAP2 (KEY_CFG_TAP1 + 1) 22 | #define KEY_CFG_TAP3 (KEY_CFG_TAP2 + 1) 23 | #define KEY_CFG_TAP_QUANTIZE (KEY_CFG_TAP3 + 1) 24 | #define KEY_CFG_TAP_JERK (KEY_CFG_TAP_QUANTIZE + 1) 25 | #define KEY_CFG_DR_INT (KEY_CFG_TAP_JERK + 1) 26 | #define KEY_CFG_AUTH (KEY_CFG_DR_INT + 1) 27 | #define KEY_CFG_TAP_SAVE_ACCB (KEY_CFG_AUTH + 1) 28 | #define KEY_CFG_TAP_CLEAR_STICKY (KEY_CFG_TAP_SAVE_ACCB + 1) 29 | #define KEY_CFG_FIFO_ON_EVENT (KEY_CFG_TAP_CLEAR_STICKY + 1) 30 | #define KEY_FCFG_ACCEL_INPUT (KEY_CFG_FIFO_ON_EVENT + 1) 31 | #define KEY_FCFG_ACCEL_INIT (KEY_FCFG_ACCEL_INPUT + 1) 32 | #define KEY_CFG_23 (KEY_FCFG_ACCEL_INIT + 1) 33 | #define KEY_FCFG_1 (KEY_CFG_23 + 1) 34 | #define KEY_FCFG_3 (KEY_FCFG_1 + 1) 35 | #define KEY_FCFG_2 (KEY_FCFG_3 + 1) 36 | #define KEY_CFG_3D (KEY_FCFG_2 + 1) 37 | #define KEY_CFG_3B (KEY_CFG_3D + 1) 38 | #define KEY_CFG_3C (KEY_CFG_3B + 1) 39 | #define KEY_FCFG_5 (KEY_CFG_3C + 1) 40 | #define KEY_FCFG_4 (KEY_FCFG_5 + 1) 41 | #define KEY_FCFG_7 (KEY_FCFG_4 + 1) 42 | #define KEY_FCFG_FSCALE (KEY_FCFG_7 + 1) 43 | #define KEY_FCFG_AZ (KEY_FCFG_FSCALE + 1) 44 | #define KEY_FCFG_6 (KEY_FCFG_AZ + 1) 45 | #define KEY_FCFG_LSB4 (KEY_FCFG_6 + 1) 46 | #define KEY_CFG_12 (KEY_FCFG_LSB4 + 1) 47 | #define KEY_CFG_14 (KEY_CFG_12 + 1) 48 | #define KEY_CFG_15 (KEY_CFG_14 + 1) 49 | #define KEY_CFG_16 (KEY_CFG_15 + 1) 50 | #define KEY_CFG_18 (KEY_CFG_16 + 1) 51 | #define KEY_CFG_6 (KEY_CFG_18 + 1) 52 | #define KEY_CFG_7 (KEY_CFG_6 + 1) 53 | #define KEY_CFG_4 (KEY_CFG_7 + 1) 54 | #define KEY_CFG_5 (KEY_CFG_4 + 1) 55 | #define KEY_CFG_2 (KEY_CFG_5 + 1) 56 | #define KEY_CFG_3 (KEY_CFG_2 + 1) 57 | #define KEY_CFG_1 (KEY_CFG_3 + 1) 58 | #define KEY_CFG_EXTERNAL (KEY_CFG_1 + 1) 59 | #define KEY_CFG_8 (KEY_CFG_EXTERNAL + 1) 60 | #define KEY_CFG_9 (KEY_CFG_8 + 1) 61 | #define KEY_CFG_ORIENT_3 (KEY_CFG_9 + 1) 62 | #define KEY_CFG_ORIENT_2 (KEY_CFG_ORIENT_3 + 1) 63 | #define KEY_CFG_ORIENT_1 (KEY_CFG_ORIENT_2 + 1) 64 | #define KEY_CFG_GYRO_SOURCE (KEY_CFG_ORIENT_1 + 1) 65 | #define KEY_CFG_ORIENT_IRQ_1 (KEY_CFG_GYRO_SOURCE + 1) 66 | #define KEY_CFG_ORIENT_IRQ_2 (KEY_CFG_ORIENT_IRQ_1 + 1) 67 | #define KEY_CFG_ORIENT_IRQ_3 (KEY_CFG_ORIENT_IRQ_2 + 1) 68 | #define KEY_FCFG_MAG_VAL (KEY_CFG_ORIENT_IRQ_3 + 1) 69 | #define KEY_FCFG_MAG_MOV (KEY_FCFG_MAG_VAL + 1) 70 | #define KEY_CFG_LP_QUAT (KEY_FCFG_MAG_MOV + 1) 71 | 72 | /* MPU6050 keys */ 73 | #define KEY_CFG_ACCEL_FILTER (KEY_CFG_LP_QUAT + 1) 74 | #define KEY_CFG_MOTION_BIAS (KEY_CFG_ACCEL_FILTER + 1) 75 | #define KEY_TEMPLABEL (KEY_CFG_MOTION_BIAS + 1) 76 | 77 | #define KEY_D_0_22 (KEY_TEMPLABEL + 1) 78 | #define KEY_D_0_24 (KEY_D_0_22 + 1) 79 | #define KEY_D_0_36 (KEY_D_0_24 + 1) 80 | #define KEY_D_0_52 (KEY_D_0_36 + 1) 81 | #define KEY_D_0_96 (KEY_D_0_52 + 1) 82 | #define KEY_D_0_104 (KEY_D_0_96 + 1) 83 | #define KEY_D_0_108 (KEY_D_0_104 + 1) 84 | #define KEY_D_0_163 (KEY_D_0_108 + 1) 85 | #define KEY_D_0_188 (KEY_D_0_163 + 1) 86 | #define KEY_D_0_192 (KEY_D_0_188 + 1) 87 | #define KEY_D_0_224 (KEY_D_0_192 + 1) 88 | #define KEY_D_0_228 (KEY_D_0_224 + 1) 89 | #define KEY_D_0_232 (KEY_D_0_228 + 1) 90 | #define KEY_D_0_236 (KEY_D_0_232 + 1) 91 | 92 | #define KEY_DMP_PREVPTAT (KEY_D_0_236 + 1) 93 | #define KEY_D_1_2 (KEY_DMP_PREVPTAT + 1) 94 | #define KEY_D_1_4 (KEY_D_1_2 + 1) 95 | #define KEY_D_1_8 (KEY_D_1_4 + 1) 96 | #define KEY_D_1_10 (KEY_D_1_8 + 1) 97 | #define KEY_D_1_24 (KEY_D_1_10 + 1) 98 | #define KEY_D_1_28 (KEY_D_1_24 + 1) 99 | #define KEY_D_1_36 (KEY_D_1_28 + 1) 100 | #define KEY_D_1_40 (KEY_D_1_36 + 1) 101 | #define KEY_D_1_44 (KEY_D_1_40 + 1) 102 | #define KEY_D_1_72 (KEY_D_1_44 + 1) 103 | #define KEY_D_1_74 (KEY_D_1_72 + 1) 104 | #define KEY_D_1_79 (KEY_D_1_74 + 1) 105 | #define KEY_D_1_88 (KEY_D_1_79 + 1) 106 | #define KEY_D_1_90 (KEY_D_1_88 + 1) 107 | #define KEY_D_1_92 (KEY_D_1_90 + 1) 108 | #define KEY_D_1_96 (KEY_D_1_92 + 1) 109 | #define KEY_D_1_98 (KEY_D_1_96 + 1) 110 | #define KEY_D_1_100 (KEY_D_1_98 + 1) 111 | #define KEY_D_1_106 (KEY_D_1_100 + 1) 112 | #define KEY_D_1_108 (KEY_D_1_106 + 1) 113 | #define KEY_D_1_112 (KEY_D_1_108 + 1) 114 | #define KEY_D_1_128 (KEY_D_1_112 + 1) 115 | #define KEY_D_1_152 (KEY_D_1_128 + 1) 116 | #define KEY_D_1_160 (KEY_D_1_152 + 1) 117 | #define KEY_D_1_168 (KEY_D_1_160 + 1) 118 | #define KEY_D_1_175 (KEY_D_1_168 + 1) 119 | #define KEY_D_1_176 (KEY_D_1_175 + 1) 120 | #define KEY_D_1_178 (KEY_D_1_176 + 1) 121 | #define KEY_D_1_179 (KEY_D_1_178 + 1) 122 | #define KEY_D_1_218 (KEY_D_1_179 + 1) 123 | #define KEY_D_1_232 (KEY_D_1_218 + 1) 124 | #define KEY_D_1_236 (KEY_D_1_232 + 1) 125 | #define KEY_D_1_240 (KEY_D_1_236 + 1) 126 | #define KEY_D_1_244 (KEY_D_1_240 + 1) 127 | #define KEY_D_1_250 (KEY_D_1_244 + 1) 128 | #define KEY_D_1_252 (KEY_D_1_250 + 1) 129 | #define KEY_D_2_12 (KEY_D_1_252 + 1) 130 | #define KEY_D_2_96 (KEY_D_2_12 + 1) 131 | #define KEY_D_2_108 (KEY_D_2_96 + 1) 132 | #define KEY_D_2_208 (KEY_D_2_108 + 1) 133 | #define KEY_FLICK_MSG (KEY_D_2_208 + 1) 134 | #define KEY_FLICK_COUNTER (KEY_FLICK_MSG + 1) 135 | #define KEY_FLICK_LOWER (KEY_FLICK_COUNTER + 1) 136 | #define KEY_CFG_FLICK_IN (KEY_FLICK_LOWER + 1) 137 | #define KEY_FLICK_UPPER (KEY_CFG_FLICK_IN + 1) 138 | #define KEY_CGNOTICE_INTR (KEY_FLICK_UPPER + 1) 139 | #define KEY_D_2_224 (KEY_CGNOTICE_INTR + 1) 140 | #define KEY_D_2_244 (KEY_D_2_224 + 1) 141 | #define KEY_D_2_248 (KEY_D_2_244 + 1) 142 | #define KEY_D_2_252 (KEY_D_2_248 + 1) 143 | 144 | #define KEY_D_GYRO_BIAS_X (KEY_D_2_252 + 1) 145 | #define KEY_D_GYRO_BIAS_Y (KEY_D_GYRO_BIAS_X + 1) 146 | #define KEY_D_GYRO_BIAS_Z (KEY_D_GYRO_BIAS_Y + 1) 147 | #define KEY_D_ACC_BIAS_X (KEY_D_GYRO_BIAS_Z + 1) 148 | #define KEY_D_ACC_BIAS_Y (KEY_D_ACC_BIAS_X + 1) 149 | #define KEY_D_ACC_BIAS_Z (KEY_D_ACC_BIAS_Y + 1) 150 | #define KEY_D_GYRO_ENABLE (KEY_D_ACC_BIAS_Z + 1) 151 | #define KEY_D_ACCEL_ENABLE (KEY_D_GYRO_ENABLE + 1) 152 | #define KEY_D_QUAT_ENABLE (KEY_D_ACCEL_ENABLE +1) 153 | #define KEY_D_OUTPUT_ENABLE (KEY_D_QUAT_ENABLE + 1) 154 | #define KEY_D_CR_TIME_G (KEY_D_OUTPUT_ENABLE + 1) 155 | #define KEY_D_CR_TIME_A (KEY_D_CR_TIME_G + 1) 156 | #define KEY_D_CR_TIME_Q (KEY_D_CR_TIME_A + 1) 157 | #define KEY_D_CS_TAX (KEY_D_CR_TIME_Q + 1) 158 | #define KEY_D_CS_TAY (KEY_D_CS_TAX + 1) 159 | #define KEY_D_CS_TAZ (KEY_D_CS_TAY + 1) 160 | #define KEY_D_CS_TGX (KEY_D_CS_TAZ + 1) 161 | #define KEY_D_CS_TGY (KEY_D_CS_TGX + 1) 162 | #define KEY_D_CS_TGZ (KEY_D_CS_TGY + 1) 163 | #define KEY_D_CS_TQ0 (KEY_D_CS_TGZ + 1) 164 | #define KEY_D_CS_TQ1 (KEY_D_CS_TQ0 + 1) 165 | #define KEY_D_CS_TQ2 (KEY_D_CS_TQ1 + 1) 166 | #define KEY_D_CS_TQ3 (KEY_D_CS_TQ2 + 1) 167 | 168 | /* Compass keys */ 169 | #define KEY_CPASS_BIAS_X (KEY_D_CS_TQ3 + 1) 170 | #define KEY_CPASS_BIAS_Y (KEY_CPASS_BIAS_X + 1) 171 | #define KEY_CPASS_BIAS_Z (KEY_CPASS_BIAS_Y + 1) 172 | #define KEY_CPASS_MTX_00 (KEY_CPASS_BIAS_Z + 1) 173 | #define KEY_CPASS_MTX_01 (KEY_CPASS_MTX_00 + 1) 174 | #define KEY_CPASS_MTX_02 (KEY_CPASS_MTX_01 + 1) 175 | #define KEY_CPASS_MTX_10 (KEY_CPASS_MTX_02 + 1) 176 | #define KEY_CPASS_MTX_11 (KEY_CPASS_MTX_10 + 1) 177 | #define KEY_CPASS_MTX_12 (KEY_CPASS_MTX_11 + 1) 178 | #define KEY_CPASS_MTX_20 (KEY_CPASS_MTX_12 + 1) 179 | #define KEY_CPASS_MTX_21 (KEY_CPASS_MTX_20 + 1) 180 | #define KEY_CPASS_MTX_22 (KEY_CPASS_MTX_21 + 1) 181 | 182 | /* Gesture Keys */ 183 | #define KEY_DMP_TAPW_MIN (KEY_CPASS_MTX_22 + 1) 184 | #define KEY_DMP_TAP_THR_X (KEY_DMP_TAPW_MIN + 1) 185 | #define KEY_DMP_TAP_THR_Y (KEY_DMP_TAP_THR_X + 1) 186 | #define KEY_DMP_TAP_THR_Z (KEY_DMP_TAP_THR_Y + 1) 187 | #define KEY_DMP_SH_TH_Y (KEY_DMP_TAP_THR_Z + 1) 188 | #define KEY_DMP_SH_TH_X (KEY_DMP_SH_TH_Y + 1) 189 | #define KEY_DMP_SH_TH_Z (KEY_DMP_SH_TH_X + 1) 190 | #define KEY_DMP_ORIENT (KEY_DMP_SH_TH_Z + 1) 191 | #define KEY_D_ACT0 (KEY_DMP_ORIENT + 1) 192 | #define KEY_D_ACSX (KEY_D_ACT0 + 1) 193 | #define KEY_D_ACSY (KEY_D_ACSX + 1) 194 | #define KEY_D_ACSZ (KEY_D_ACSY + 1) 195 | 196 | #define KEY_X_GRT_Y_TMP (KEY_D_ACSZ + 1) 197 | #define KEY_SKIP_X_GRT_Y_TMP (KEY_X_GRT_Y_TMP + 1) 198 | #define KEY_SKIP_END_COMPARE (KEY_SKIP_X_GRT_Y_TMP + 1) 199 | #define KEY_END_COMPARE_Y_X_TMP2 (KEY_SKIP_END_COMPARE + 1) 200 | #define KEY_CFG_ANDROID_ORIENT_INT (KEY_END_COMPARE_Y_X_TMP2 + 1) 201 | #define KEY_NO_ORIENT_INTERRUPT (KEY_CFG_ANDROID_ORIENT_INT + 1) 202 | #define KEY_END_COMPARE_Y_X_TMP (KEY_NO_ORIENT_INTERRUPT + 1) 203 | #define KEY_END_ORIENT_1 (KEY_END_COMPARE_Y_X_TMP + 1) 204 | #define KEY_END_COMPARE_Y_X (KEY_END_ORIENT_1 + 1) 205 | #define KEY_END_ORIENT (KEY_END_COMPARE_Y_X + 1) 206 | #define KEY_X_GRT_Y (KEY_END_ORIENT + 1) 207 | #define KEY_NOT_TIME_MINUS_1 (KEY_X_GRT_Y + 1) 208 | #define KEY_END_COMPARE_Y_X_TMP3 (KEY_NOT_TIME_MINUS_1 + 1) 209 | #define KEY_X_GRT_Y_TMP2 (KEY_END_COMPARE_Y_X_TMP3 + 1) 210 | 211 | /* Authenticate Keys */ 212 | #define KEY_D_AUTH_OUT (KEY_X_GRT_Y_TMP2 + 1) 213 | #define KEY_D_AUTH_IN (KEY_D_AUTH_OUT + 1) 214 | #define KEY_D_AUTH_A (KEY_D_AUTH_IN + 1) 215 | #define KEY_D_AUTH_B (KEY_D_AUTH_A + 1) 216 | 217 | /* Pedometer standalone only keys */ 218 | #define KEY_D_PEDSTD_BP_B (KEY_D_AUTH_B + 1) 219 | #define KEY_D_PEDSTD_HP_A (KEY_D_PEDSTD_BP_B + 1) 220 | #define KEY_D_PEDSTD_HP_B (KEY_D_PEDSTD_HP_A + 1) 221 | #define KEY_D_PEDSTD_BP_A4 (KEY_D_PEDSTD_HP_B + 1) 222 | #define KEY_D_PEDSTD_BP_A3 (KEY_D_PEDSTD_BP_A4 + 1) 223 | #define KEY_D_PEDSTD_BP_A2 (KEY_D_PEDSTD_BP_A3 + 1) 224 | #define KEY_D_PEDSTD_BP_A1 (KEY_D_PEDSTD_BP_A2 + 1) 225 | #define KEY_D_PEDSTD_INT_THRSH (KEY_D_PEDSTD_BP_A1 + 1) 226 | #define KEY_D_PEDSTD_CLIP (KEY_D_PEDSTD_INT_THRSH + 1) 227 | #define KEY_D_PEDSTD_SB (KEY_D_PEDSTD_CLIP + 1) 228 | #define KEY_D_PEDSTD_SB_TIME (KEY_D_PEDSTD_SB + 1) 229 | #define KEY_D_PEDSTD_PEAKTHRSH (KEY_D_PEDSTD_SB_TIME + 1) 230 | #define KEY_D_PEDSTD_TIML (KEY_D_PEDSTD_PEAKTHRSH + 1) 231 | #define KEY_D_PEDSTD_TIMH (KEY_D_PEDSTD_TIML + 1) 232 | #define KEY_D_PEDSTD_PEAK (KEY_D_PEDSTD_TIMH + 1) 233 | #define KEY_D_PEDSTD_TIMECTR (KEY_D_PEDSTD_PEAK + 1) 234 | #define KEY_D_PEDSTD_STEPCTR (KEY_D_PEDSTD_TIMECTR + 1) 235 | #define KEY_D_PEDSTD_WALKTIME (KEY_D_PEDSTD_STEPCTR + 1) 236 | #define KEY_D_PEDSTD_DECI (KEY_D_PEDSTD_WALKTIME + 1) 237 | 238 | /*Host Based No Motion*/ 239 | #define KEY_D_HOST_NO_MOT (KEY_D_PEDSTD_DECI + 1) 240 | 241 | /* EIS keys */ 242 | #define KEY_P_EIS_FIFO_FOOTER (KEY_D_HOST_NO_MOT + 1) 243 | #define KEY_P_EIS_FIFO_YSHIFT (KEY_P_EIS_FIFO_FOOTER + 1) 244 | #define KEY_P_EIS_DATA_RATE (KEY_P_EIS_FIFO_YSHIFT + 1) 245 | #define KEY_P_EIS_FIFO_XSHIFT (KEY_P_EIS_DATA_RATE + 1) 246 | #define KEY_P_EIS_FIFO_SYNC (KEY_P_EIS_FIFO_XSHIFT + 1) 247 | #define KEY_P_EIS_FIFO_ZSHIFT (KEY_P_EIS_FIFO_SYNC + 1) 248 | #define KEY_P_EIS_FIFO_READY (KEY_P_EIS_FIFO_ZSHIFT + 1) 249 | #define KEY_DMP_FOOTER (KEY_P_EIS_FIFO_READY + 1) 250 | #define KEY_DMP_INTX_HC (KEY_DMP_FOOTER + 1) 251 | #define KEY_DMP_INTX_PH (KEY_DMP_INTX_HC + 1) 252 | #define KEY_DMP_INTX_SH (KEY_DMP_INTX_PH + 1) 253 | #define KEY_DMP_AINV_SH (KEY_DMP_INTX_SH + 1) 254 | #define KEY_DMP_A_INV_XH (KEY_DMP_AINV_SH + 1) 255 | #define KEY_DMP_AINV_PH (KEY_DMP_A_INV_XH + 1) 256 | #define KEY_DMP_CTHX_H (KEY_DMP_AINV_PH + 1) 257 | #define KEY_DMP_CTHY_H (KEY_DMP_CTHX_H + 1) 258 | #define KEY_DMP_CTHZ_H (KEY_DMP_CTHY_H + 1) 259 | #define KEY_DMP_NCTHX_H (KEY_DMP_CTHZ_H + 1) 260 | #define KEY_DMP_NCTHY_H (KEY_DMP_NCTHX_H + 1) 261 | #define KEY_DMP_NCTHZ_H (KEY_DMP_NCTHY_H + 1) 262 | #define KEY_DMP_CTSQ_XH (KEY_DMP_NCTHZ_H + 1) 263 | #define KEY_DMP_CTSQ_YH (KEY_DMP_CTSQ_XH + 1) 264 | #define KEY_DMP_CTSQ_ZH (KEY_DMP_CTSQ_YH + 1) 265 | #define KEY_DMP_INTX_H (KEY_DMP_CTSQ_ZH + 1) 266 | #define KEY_DMP_INTY_H (KEY_DMP_INTX_H + 1) 267 | #define KEY_DMP_INTZ_H (KEY_DMP_INTY_H + 1) 268 | //#define KEY_DMP_HPX_H (KEY_DMP_INTZ_H + 1) 269 | //#define KEY_DMP_HPY_H (KEY_DMP_HPX_H + 1) 270 | //#define KEY_DMP_HPZ_H (KEY_DMP_HPY_H + 1) 271 | 272 | /* Stream keys */ 273 | #define KEY_STREAM_P_GYRO_Z (KEY_DMP_INTZ_H + 1) 274 | #define KEY_STREAM_P_GYRO_Y (KEY_STREAM_P_GYRO_Z + 1) 275 | #define KEY_STREAM_P_GYRO_X (KEY_STREAM_P_GYRO_Y + 1) 276 | #define KEY_STREAM_P_TEMP (KEY_STREAM_P_GYRO_X + 1) 277 | #define KEY_STREAM_P_AUX_Y (KEY_STREAM_P_TEMP + 1) 278 | #define KEY_STREAM_P_AUX_X (KEY_STREAM_P_AUX_Y + 1) 279 | #define KEY_STREAM_P_AUX_Z (KEY_STREAM_P_AUX_X + 1) 280 | #define KEY_STREAM_P_ACCEL_Y (KEY_STREAM_P_AUX_Z + 1) 281 | #define KEY_STREAM_P_ACCEL_X (KEY_STREAM_P_ACCEL_Y + 1) 282 | #define KEY_STREAM_P_FOOTER (KEY_STREAM_P_ACCEL_X + 1) 283 | #define KEY_STREAM_P_ACCEL_Z (KEY_STREAM_P_FOOTER + 1) 284 | 285 | #define NUM_KEYS (KEY_STREAM_P_ACCEL_Z + 1) 286 | 287 | typedef struct { 288 | unsigned short key; 289 | unsigned short addr; 290 | } tKeyLabel; 291 | 292 | #define DINA0A 0x0a 293 | #define DINA22 0x22 294 | #define DINA42 0x42 295 | #define DINA5A 0x5a 296 | 297 | #define DINA06 0x06 298 | #define DINA0E 0x0e 299 | #define DINA16 0x16 300 | #define DINA1E 0x1e 301 | #define DINA26 0x26 302 | #define DINA2E 0x2e 303 | #define DINA36 0x36 304 | #define DINA3E 0x3e 305 | #define DINA46 0x46 306 | #define DINA4E 0x4e 307 | #define DINA56 0x56 308 | #define DINA5E 0x5e 309 | #define DINA66 0x66 310 | #define DINA6E 0x6e 311 | #define DINA76 0x76 312 | #define DINA7E 0x7e 313 | 314 | #define DINA00 0x00 315 | #define DINA08 0x08 316 | #define DINA10 0x10 317 | #define DINA18 0x18 318 | #define DINA20 0x20 319 | #define DINA28 0x28 320 | #define DINA30 0x30 321 | #define DINA38 0x38 322 | #define DINA40 0x40 323 | #define DINA48 0x48 324 | #define DINA50 0x50 325 | #define DINA58 0x58 326 | #define DINA60 0x60 327 | #define DINA68 0x68 328 | #define DINA70 0x70 329 | #define DINA78 0x78 330 | 331 | #define DINA04 0x04 332 | #define DINA0C 0x0c 333 | #define DINA14 0x14 334 | #define DINA1C 0x1C 335 | #define DINA24 0x24 336 | #define DINA2C 0x2c 337 | #define DINA34 0x34 338 | #define DINA3C 0x3c 339 | #define DINA44 0x44 340 | #define DINA4C 0x4c 341 | #define DINA54 0x54 342 | #define DINA5C 0x5c 343 | #define DINA64 0x64 344 | #define DINA6C 0x6c 345 | #define DINA74 0x74 346 | #define DINA7C 0x7c 347 | 348 | #define DINA01 0x01 349 | #define DINA09 0x09 350 | #define DINA11 0x11 351 | #define DINA19 0x19 352 | #define DINA21 0x21 353 | #define DINA29 0x29 354 | #define DINA31 0x31 355 | #define DINA39 0x39 356 | #define DINA41 0x41 357 | #define DINA49 0x49 358 | #define DINA51 0x51 359 | #define DINA59 0x59 360 | #define DINA61 0x61 361 | #define DINA69 0x69 362 | #define DINA71 0x71 363 | #define DINA79 0x79 364 | 365 | #define DINA25 0x25 366 | #define DINA2D 0x2d 367 | #define DINA35 0x35 368 | #define DINA3D 0x3d 369 | #define DINA4D 0x4d 370 | #define DINA55 0x55 371 | #define DINA5D 0x5D 372 | #define DINA6D 0x6d 373 | #define DINA75 0x75 374 | #define DINA7D 0x7d 375 | 376 | #define DINADC 0xdc 377 | #define DINAF2 0xf2 378 | #define DINAAB 0xab 379 | #define DINAAA 0xaa 380 | #define DINAF1 0xf1 381 | #define DINADF 0xdf 382 | #define DINADA 0xda 383 | #define DINAB1 0xb1 384 | #define DINAB9 0xb9 385 | #define DINAF3 0xf3 386 | #define DINA8B 0x8b 387 | #define DINAA3 0xa3 388 | #define DINA91 0x91 389 | #define DINAB6 0xb6 390 | #define DINAB4 0xb4 391 | 392 | 393 | #define DINC00 0x00 394 | #define DINC01 0x01 395 | #define DINC02 0x02 396 | #define DINC03 0x03 397 | #define DINC08 0x08 398 | #define DINC09 0x09 399 | #define DINC0A 0x0a 400 | #define DINC0B 0x0b 401 | #define DINC10 0x10 402 | #define DINC11 0x11 403 | #define DINC12 0x12 404 | #define DINC13 0x13 405 | #define DINC18 0x18 406 | #define DINC19 0x19 407 | #define DINC1A 0x1a 408 | #define DINC1B 0x1b 409 | 410 | #define DINC20 0x20 411 | #define DINC21 0x21 412 | #define DINC22 0x22 413 | #define DINC23 0x23 414 | #define DINC28 0x28 415 | #define DINC29 0x29 416 | #define DINC2A 0x2a 417 | #define DINC2B 0x2b 418 | #define DINC30 0x30 419 | #define DINC31 0x31 420 | #define DINC32 0x32 421 | #define DINC33 0x33 422 | #define DINC38 0x38 423 | #define DINC39 0x39 424 | #define DINC3A 0x3a 425 | #define DINC3B 0x3b 426 | 427 | #define DINC40 0x40 428 | #define DINC41 0x41 429 | #define DINC42 0x42 430 | #define DINC43 0x43 431 | #define DINC48 0x48 432 | #define DINC49 0x49 433 | #define DINC4A 0x4a 434 | #define DINC4B 0x4b 435 | #define DINC50 0x50 436 | #define DINC51 0x51 437 | #define DINC52 0x52 438 | #define DINC53 0x53 439 | #define DINC58 0x58 440 | #define DINC59 0x59 441 | #define DINC5A 0x5a 442 | #define DINC5B 0x5b 443 | 444 | #define DINC60 0x60 445 | #define DINC61 0x61 446 | #define DINC62 0x62 447 | #define DINC63 0x63 448 | #define DINC68 0x68 449 | #define DINC69 0x69 450 | #define DINC6A 0x6a 451 | #define DINC6B 0x6b 452 | #define DINC70 0x70 453 | #define DINC71 0x71 454 | #define DINC72 0x72 455 | #define DINC73 0x73 456 | #define DINC78 0x78 457 | #define DINC79 0x79 458 | #define DINC7A 0x7a 459 | #define DINC7B 0x7b 460 | 461 | #define DIND40 0x40 462 | 463 | 464 | #define DINA80 0x80 465 | #define DINA90 0x90 466 | #define DINAA0 0xa0 467 | #define DINAC9 0xc9 468 | #define DINACB 0xcb 469 | #define DINACD 0xcd 470 | #define DINACF 0xcf 471 | #define DINAC8 0xc8 472 | #define DINACA 0xca 473 | #define DINACC 0xcc 474 | #define DINACE 0xce 475 | #define DINAD8 0xd8 476 | #define DINADD 0xdd 477 | #define DINAF8 0xf0 478 | #define DINAFE 0xfe 479 | 480 | #define DINBF8 0xf8 481 | #define DINAC0 0xb0 482 | #define DINAC1 0xb1 483 | #define DINAC2 0xb4 484 | #define DINAC3 0xb5 485 | #define DINAC4 0xb8 486 | #define DINAC5 0xb9 487 | #define DINBC0 0xc0 488 | #define DINBC2 0xc2 489 | #define DINBC4 0xc4 490 | #define DINBC6 0xc6 491 | 492 | 493 | 494 | #endif // DMPKEY_H__ 495 | -------------------------------------------------------------------------------- /src/util/dmpmap.h: -------------------------------------------------------------------------------- 1 | /* 2 | $License: 3 | Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. 4 | $ 5 | */ 6 | #ifndef DMPMAP_H 7 | #define DMPMAP_H 8 | 9 | #ifdef __cplusplus 10 | extern "C" 11 | { 12 | #endif 13 | 14 | #define DMP_PTAT 0 15 | #define DMP_XGYR 2 16 | #define DMP_YGYR 4 17 | #define DMP_ZGYR 6 18 | #define DMP_XACC 8 19 | #define DMP_YACC 10 20 | #define DMP_ZACC 12 21 | #define DMP_ADC1 14 22 | #define DMP_ADC2 16 23 | #define DMP_ADC3 18 24 | #define DMP_BIASUNC 20 25 | #define DMP_FIFORT 22 26 | #define DMP_INVGSFH 24 27 | #define DMP_INVGSFL 26 28 | #define DMP_1H 28 29 | #define DMP_1L 30 30 | #define DMP_BLPFSTCH 32 31 | #define DMP_BLPFSTCL 34 32 | #define DMP_BLPFSXH 36 33 | #define DMP_BLPFSXL 38 34 | #define DMP_BLPFSYH 40 35 | #define DMP_BLPFSYL 42 36 | #define DMP_BLPFSZH 44 37 | #define DMP_BLPFSZL 46 38 | #define DMP_BLPFMTC 48 39 | #define DMP_SMC 50 40 | #define DMP_BLPFMXH 52 41 | #define DMP_BLPFMXL 54 42 | #define DMP_BLPFMYH 56 43 | #define DMP_BLPFMYL 58 44 | #define DMP_BLPFMZH 60 45 | #define DMP_BLPFMZL 62 46 | #define DMP_BLPFC 64 47 | #define DMP_SMCTH 66 48 | #define DMP_0H2 68 49 | #define DMP_0L2 70 50 | #define DMP_BERR2H 72 51 | #define DMP_BERR2L 74 52 | #define DMP_BERR2NH 76 53 | #define DMP_SMCINC 78 54 | #define DMP_ANGVBXH 80 55 | #define DMP_ANGVBXL 82 56 | #define DMP_ANGVBYH 84 57 | #define DMP_ANGVBYL 86 58 | #define DMP_ANGVBZH 88 59 | #define DMP_ANGVBZL 90 60 | #define DMP_BERR1H 92 61 | #define DMP_BERR1L 94 62 | #define DMP_ATCH 96 63 | #define DMP_BIASUNCSF 98 64 | #define DMP_ACT2H 100 65 | #define DMP_ACT2L 102 66 | #define DMP_GSFH 104 67 | #define DMP_GSFL 106 68 | #define DMP_GH 108 69 | #define DMP_GL 110 70 | #define DMP_0_5H 112 71 | #define DMP_0_5L 114 72 | #define DMP_0_0H 116 73 | #define DMP_0_0L 118 74 | #define DMP_1_0H 120 75 | #define DMP_1_0L 122 76 | #define DMP_1_5H 124 77 | #define DMP_1_5L 126 78 | #define DMP_TMP1AH 128 79 | #define DMP_TMP1AL 130 80 | #define DMP_TMP2AH 132 81 | #define DMP_TMP2AL 134 82 | #define DMP_TMP3AH 136 83 | #define DMP_TMP3AL 138 84 | #define DMP_TMP4AH 140 85 | #define DMP_TMP4AL 142 86 | #define DMP_XACCW 144 87 | #define DMP_TMP5 146 88 | #define DMP_XACCB 148 89 | #define DMP_TMP8 150 90 | #define DMP_YACCB 152 91 | #define DMP_TMP9 154 92 | #define DMP_ZACCB 156 93 | #define DMP_TMP10 158 94 | #define DMP_DZH 160 95 | #define DMP_DZL 162 96 | #define DMP_XGCH 164 97 | #define DMP_XGCL 166 98 | #define DMP_YGCH 168 99 | #define DMP_YGCL 170 100 | #define DMP_ZGCH 172 101 | #define DMP_ZGCL 174 102 | #define DMP_YACCW 176 103 | #define DMP_TMP7 178 104 | #define DMP_AFB1H 180 105 | #define DMP_AFB1L 182 106 | #define DMP_AFB2H 184 107 | #define DMP_AFB2L 186 108 | #define DMP_MAGFBH 188 109 | #define DMP_MAGFBL 190 110 | #define DMP_QT1H 192 111 | #define DMP_QT1L 194 112 | #define DMP_QT2H 196 113 | #define DMP_QT2L 198 114 | #define DMP_QT3H 200 115 | #define DMP_QT3L 202 116 | #define DMP_QT4H 204 117 | #define DMP_QT4L 206 118 | #define DMP_CTRL1H 208 119 | #define DMP_CTRL1L 210 120 | #define DMP_CTRL2H 212 121 | #define DMP_CTRL2L 214 122 | #define DMP_CTRL3H 216 123 | #define DMP_CTRL3L 218 124 | #define DMP_CTRL4H 220 125 | #define DMP_CTRL4L 222 126 | #define DMP_CTRLS1 224 127 | #define DMP_CTRLSF1 226 128 | #define DMP_CTRLS2 228 129 | #define DMP_CTRLSF2 230 130 | #define DMP_CTRLS3 232 131 | #define DMP_CTRLSFNLL 234 132 | #define DMP_CTRLS4 236 133 | #define DMP_CTRLSFNL2 238 134 | #define DMP_CTRLSFNL 240 135 | #define DMP_TMP30 242 136 | #define DMP_CTRLSFJT 244 137 | #define DMP_TMP31 246 138 | #define DMP_TMP11 248 139 | #define DMP_CTRLSF2_2 250 140 | #define DMP_TMP12 252 141 | #define DMP_CTRLSF1_2 254 142 | #define DMP_PREVPTAT 256 143 | #define DMP_ACCZB 258 144 | #define DMP_ACCXB 264 145 | #define DMP_ACCYB 266 146 | #define DMP_1HB 272 147 | #define DMP_1LB 274 148 | #define DMP_0H 276 149 | #define DMP_0L 278 150 | #define DMP_ASR22H 280 151 | #define DMP_ASR22L 282 152 | #define DMP_ASR6H 284 153 | #define DMP_ASR6L 286 154 | #define DMP_TMP13 288 155 | #define DMP_TMP14 290 156 | #define DMP_FINTXH 292 157 | #define DMP_FINTXL 294 158 | #define DMP_FINTYH 296 159 | #define DMP_FINTYL 298 160 | #define DMP_FINTZH 300 161 | #define DMP_FINTZL 302 162 | #define DMP_TMP1BH 304 163 | #define DMP_TMP1BL 306 164 | #define DMP_TMP2BH 308 165 | #define DMP_TMP2BL 310 166 | #define DMP_TMP3BH 312 167 | #define DMP_TMP3BL 314 168 | #define DMP_TMP4BH 316 169 | #define DMP_TMP4BL 318 170 | #define DMP_STXG 320 171 | #define DMP_ZCTXG 322 172 | #define DMP_STYG 324 173 | #define DMP_ZCTYG 326 174 | #define DMP_STZG 328 175 | #define DMP_ZCTZG 330 176 | #define DMP_CTRLSFJT2 332 177 | #define DMP_CTRLSFJTCNT 334 178 | #define DMP_PVXG 336 179 | #define DMP_TMP15 338 180 | #define DMP_PVYG 340 181 | #define DMP_TMP16 342 182 | #define DMP_PVZG 344 183 | #define DMP_TMP17 346 184 | #define DMP_MNMFLAGH 352 185 | #define DMP_MNMFLAGL 354 186 | #define DMP_MNMTMH 356 187 | #define DMP_MNMTML 358 188 | #define DMP_MNMTMTHRH 360 189 | #define DMP_MNMTMTHRL 362 190 | #define DMP_MNMTHRH 364 191 | #define DMP_MNMTHRL 366 192 | #define DMP_ACCQD4H 368 193 | #define DMP_ACCQD4L 370 194 | #define DMP_ACCQD5H 372 195 | #define DMP_ACCQD5L 374 196 | #define DMP_ACCQD6H 376 197 | #define DMP_ACCQD6L 378 198 | #define DMP_ACCQD7H 380 199 | #define DMP_ACCQD7L 382 200 | #define DMP_ACCQD0H 384 201 | #define DMP_ACCQD0L 386 202 | #define DMP_ACCQD1H 388 203 | #define DMP_ACCQD1L 390 204 | #define DMP_ACCQD2H 392 205 | #define DMP_ACCQD2L 394 206 | #define DMP_ACCQD3H 396 207 | #define DMP_ACCQD3L 398 208 | #define DMP_XN2H 400 209 | #define DMP_XN2L 402 210 | #define DMP_XN1H 404 211 | #define DMP_XN1L 406 212 | #define DMP_YN2H 408 213 | #define DMP_YN2L 410 214 | #define DMP_YN1H 412 215 | #define DMP_YN1L 414 216 | #define DMP_YH 416 217 | #define DMP_YL 418 218 | #define DMP_B0H 420 219 | #define DMP_B0L 422 220 | #define DMP_A1H 424 221 | #define DMP_A1L 426 222 | #define DMP_A2H 428 223 | #define DMP_A2L 430 224 | #define DMP_SEM1 432 225 | #define DMP_FIFOCNT 434 226 | #define DMP_SH_TH_X 436 227 | #define DMP_PACKET 438 228 | #define DMP_SH_TH_Y 440 229 | #define DMP_FOOTER 442 230 | #define DMP_SH_TH_Z 444 231 | #define DMP_TEMP29 448 232 | #define DMP_TEMP30 450 233 | #define DMP_XACCB_PRE 452 234 | #define DMP_XACCB_PREL 454 235 | #define DMP_YACCB_PRE 456 236 | #define DMP_YACCB_PREL 458 237 | #define DMP_ZACCB_PRE 460 238 | #define DMP_ZACCB_PREL 462 239 | #define DMP_TMP22 464 240 | #define DMP_TAP_TIMER 466 241 | #define DMP_TAP_THX 468 242 | #define DMP_TAP_THY 472 243 | #define DMP_TAP_THZ 476 244 | #define DMP_TAPW_MIN 478 245 | #define DMP_TMP25 480 246 | #define DMP_TMP26 482 247 | #define DMP_TMP27 484 248 | #define DMP_TMP28 486 249 | #define DMP_ORIENT 488 250 | #define DMP_THRSH 490 251 | #define DMP_ENDIANH 492 252 | #define DMP_ENDIANL 494 253 | #define DMP_BLPFNMTCH 496 254 | #define DMP_BLPFNMTCL 498 255 | #define DMP_BLPFNMXH 500 256 | #define DMP_BLPFNMXL 502 257 | #define DMP_BLPFNMYH 504 258 | #define DMP_BLPFNMYL 506 259 | #define DMP_BLPFNMZH 508 260 | #define DMP_BLPFNMZL 510 261 | #ifdef __cplusplus 262 | } 263 | #endif 264 | #endif // DMPMAP_H 265 | -------------------------------------------------------------------------------- /src/util/inv_mpu.h: -------------------------------------------------------------------------------- 1 | /* 2 | $License: 3 | Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. 4 | See included License.txt for License information. 5 | $ 6 | */ 7 | /** 8 | * @addtogroup DRIVERS Sensor Driver Layer 9 | * @brief Hardware drivers to communicate with sensors via I2C. 10 | * 11 | * @{ 12 | * @file inv_mpu.h 13 | * @brief An I2C-based driver for Invensense gyroscopes. 14 | * @details This driver currently works for the following devices: 15 | * MPU6050 16 | * MPU6500 17 | * MPU9150 (or MPU6050 w/ AK8975 on the auxiliary bus) 18 | * MPU9250 (or MPU6500 w/ AK8963 on the auxiliary bus) 19 | */ 20 | 21 | #ifndef _INV_MPU_H_ 22 | #define _INV_MPU_H_ 23 | 24 | #define INV_X_GYRO (0x40) 25 | #define INV_Y_GYRO (0x20) 26 | #define INV_Z_GYRO (0x10) 27 | #define INV_XYZ_GYRO (INV_X_GYRO | INV_Y_GYRO | INV_Z_GYRO) 28 | #define INV_XYZ_ACCEL (0x08) 29 | #define INV_XYZ_COMPASS (0x01) 30 | 31 | struct int_param_s { 32 | #if defined EMPL_TARGET_MSP430 || defined MOTION_DRIVER_TARGET_MSP430 33 | void (*cb)(void); 34 | unsigned short pin; 35 | unsigned char lp_exit; 36 | unsigned char active_low; 37 | #elif defined EMPL_TARGET_UC3L0 38 | unsigned long pin; 39 | void (*cb)(volatile void*); 40 | void *arg; 41 | #elif defined EMPL_TARGET_STM32F4 42 | void (*cb)(void); 43 | #endif 44 | }; 45 | 46 | #define MPU_INT_STATUS_DATA_READY (0x0001) 47 | #define MPU_INT_STATUS_DMP (0x0002) 48 | #define MPU_INT_STATUS_PLL_READY (0x0004) 49 | #define MPU_INT_STATUS_I2C_MST (0x0008) 50 | #define MPU_INT_STATUS_FIFO_OVERFLOW (0x0010) 51 | #define MPU_INT_STATUS_ZMOT (0x0020) 52 | #define MPU_INT_STATUS_MOT (0x0040) 53 | #define MPU_INT_STATUS_FREE_FALL (0x0080) 54 | #define MPU_INT_STATUS_DMP_0 (0x0100) 55 | #define MPU_INT_STATUS_DMP_1 (0x0200) 56 | #define MPU_INT_STATUS_DMP_2 (0x0400) 57 | #define MPU_INT_STATUS_DMP_3 (0x0800) 58 | #define MPU_INT_STATUS_DMP_4 (0x1000) 59 | #define MPU_INT_STATUS_DMP_5 (0x2000) 60 | 61 | /* Set up APIs */ 62 | int set_int_enable(unsigned char enable); 63 | int mpu_init(struct int_param_s *int_param); 64 | int mpu_init_slave(void); 65 | int mpu_set_bypass(unsigned char bypass_on); 66 | 67 | /* Configuration APIs */ 68 | int mpu_lp_accel_mode(unsigned short rate); 69 | int mpu_lp_motion_interrupt(unsigned short thresh, unsigned char time, 70 | unsigned short lpa_freq); 71 | int mpu_set_int_level(unsigned char active_low); 72 | int mpu_set_int_latched(unsigned char enable); 73 | 74 | int mpu_set_dmp_state(unsigned char enable); 75 | int mpu_get_dmp_state(unsigned char *enabled); 76 | 77 | int mpu_get_lpf(unsigned short *lpf); 78 | int mpu_set_lpf(unsigned short lpf); 79 | 80 | int mpu_get_gyro_fsr(unsigned short *fsr); 81 | int mpu_set_gyro_fsr(unsigned short fsr); 82 | 83 | int mpu_get_accel_fsr(unsigned char *fsr); 84 | int mpu_set_accel_fsr(unsigned char fsr); 85 | 86 | int mpu_get_compass_fsr(unsigned short *fsr); 87 | 88 | int mpu_get_gyro_sens(float *sens); 89 | int mpu_get_accel_sens(unsigned short *sens); 90 | 91 | int mpu_get_sample_rate(unsigned short *rate); 92 | int mpu_set_sample_rate(unsigned short rate); 93 | int mpu_get_compass_sample_rate(unsigned short *rate); 94 | int mpu_set_compass_sample_rate(unsigned short rate); 95 | 96 | int mpu_get_fifo_config(unsigned char *sensors); 97 | int mpu_configure_fifo(unsigned char sensors); 98 | 99 | int mpu_get_power_state(unsigned char *power_on); 100 | int mpu_set_sensors(unsigned char sensors); 101 | 102 | int mpu_read_6500_accel_bias(long *accel_bias); 103 | int mpu_set_gyro_bias_reg(long * gyro_bias); 104 | int mpu_set_accel_bias_6500_reg(const long *accel_bias); 105 | int mpu_read_6050_accel_bias(long *accel_bias); 106 | int mpu_set_accel_bias_6050_reg(const long *accel_bias); 107 | 108 | /* Data getter/setter APIs */ 109 | int mpu_get_gyro_reg(short *data, unsigned long *timestamp); 110 | int mpu_get_accel_reg(short *data, unsigned long *timestamp); 111 | int mpu_get_compass_reg(short *data, unsigned long *timestamp); 112 | int mpu_get_temperature(long *data, unsigned long *timestamp); 113 | 114 | int mpu_get_int_status(short *status); 115 | int mpu_read_fifo(short *gyro, short *accel, unsigned long *timestamp, 116 | unsigned char *sensors, unsigned char *more); 117 | int mpu_read_fifo_stream(unsigned short length, unsigned char *data, 118 | unsigned char *more); 119 | int mpu_reset_fifo(void); 120 | 121 | int mpu_write_mem(unsigned short mem_addr, unsigned short length, 122 | unsigned char *data); 123 | int mpu_read_mem(unsigned short mem_addr, unsigned short length, 124 | unsigned char *data); 125 | int mpu_load_firmware(unsigned short length, const unsigned char *firmware, 126 | unsigned short start_addr, unsigned short sample_rate); 127 | 128 | int mpu_reg_dump(void); 129 | int mpu_read_reg(unsigned char reg, unsigned char *data); 130 | int mpu_run_self_test(long *gyro, long *accel); 131 | int mpu_run_6500_self_test(long *gyro, long *accel, unsigned char debug); 132 | int mpu_register_tap_cb(void (*func)(unsigned char, unsigned char)); 133 | 134 | #endif /* #ifndef _INV_MPU_H_ */ 135 | 136 | -------------------------------------------------------------------------------- /src/util/inv_mpu_dmp_motion_driver.c: -------------------------------------------------------------------------------- 1 | /* 2 | $License: 3 | Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. 4 | See included License.txt for License information. 5 | $ 6 | */ 7 | /** 8 | * @addtogroup DRIVERS Sensor Driver Layer 9 | * @brief Hardware drivers to communicate with sensors via I2C. 10 | * 11 | * @{ 12 | * @file inv_mpu_dmp_motion_driver.c 13 | * @brief DMP image and interface functions. 14 | * @details All functions are preceded by the dmp_ prefix to 15 | * differentiate among MPL and general driver function calls. 16 | */ 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include "inv_mpu.h" 23 | #include "inv_mpu_dmp_motion_driver.h" 24 | #include "dmpKey.h" 25 | #include "dmpmap.h" 26 | 27 | /* The following functions must be defined for this platform: 28 | * i2c_write(unsigned char slave_addr, unsigned char reg_addr, 29 | * unsigned char length, unsigned char const *data) 30 | * i2c_read(unsigned char slave_addr, unsigned char reg_addr, 31 | * unsigned char length, unsigned char *data) 32 | * delay_ms(unsigned long num_ms) 33 | * get_ms(unsigned long *count) 34 | */ 35 | #include 36 | #include "arduino_mpu9250_i2c.h" 37 | #include "arduino_mpu9250_clk.h" 38 | #define i2c_write(a, b, c, d) arduino_i2c_write(a, b, c, d) 39 | #define i2c_read(a, b, c, d) arduino_i2c_read(a, b, c, d) 40 | #define delay_ms arduino_delay_ms 41 | #define get_ms arduino_get_clock_ms 42 | #define log_i _MLPrintLog 43 | #define log_e _MLPrintLog 44 | 45 | /* These defines are copied from dmpDefaultMPU6050.c in the general MPL 46 | * releases. These defines may change for each DMP image, so be sure to modify 47 | * these values when switching to a new image. 48 | */ 49 | #define CFG_LP_QUAT (2712) 50 | #define END_ORIENT_TEMP (1866) 51 | #define CFG_27 (2742) 52 | #define CFG_20 (2224) 53 | #define CFG_23 (2745) 54 | #define CFG_FIFO_ON_EVENT (2690) 55 | #define END_PREDICTION_UPDATE (1761) 56 | #define CGNOTICE_INTR (2620) 57 | #define X_GRT_Y_TMP (1358) 58 | #define CFG_DR_INT (1029) 59 | #define CFG_AUTH (1035) 60 | #define UPDATE_PROP_ROT (1835) 61 | #define END_COMPARE_Y_X_TMP2 (1455) 62 | #define SKIP_X_GRT_Y_TMP (1359) 63 | #define SKIP_END_COMPARE (1435) 64 | #define FCFG_3 (1088) 65 | #define FCFG_2 (1066) 66 | #define FCFG_1 (1062) 67 | #define END_COMPARE_Y_X_TMP3 (1434) 68 | #define FCFG_7 (1073) 69 | #define FCFG_6 (1106) 70 | #define FLAT_STATE_END (1713) 71 | #define SWING_END_4 (1616) 72 | #define SWING_END_2 (1565) 73 | #define SWING_END_3 (1587) 74 | #define SWING_END_1 (1550) 75 | #define CFG_8 (2718) 76 | #define CFG_15 (2727) 77 | #define CFG_16 (2746) 78 | #define CFG_EXT_GYRO_BIAS (1189) 79 | #define END_COMPARE_Y_X_TMP (1407) 80 | #define DO_NOT_UPDATE_PROP_ROT (1839) 81 | #define CFG_7 (1205) 82 | #define FLAT_STATE_END_TEMP (1683) 83 | #define END_COMPARE_Y_X (1484) 84 | #define SKIP_SWING_END_1 (1551) 85 | #define SKIP_SWING_END_3 (1588) 86 | #define SKIP_SWING_END_2 (1566) 87 | #define TILTG75_START (1672) 88 | #define CFG_6 (2753) 89 | #define TILTL75_END (1669) 90 | #define END_ORIENT (1884) 91 | #define CFG_FLICK_IN (2573) 92 | #define TILTL75_START (1643) 93 | #define CFG_MOTION_BIAS (1208) 94 | #define X_GRT_Y (1408) 95 | #define TEMPLABEL (2324) 96 | #define CFG_ANDROID_ORIENT_INT (1853) 97 | #define CFG_GYRO_RAW_DATA (2722) 98 | #define X_GRT_Y_TMP2 (1379) 99 | 100 | #define D_0_22 (22+512) 101 | #define D_0_24 (24+512) 102 | 103 | #define D_0_36 (36) 104 | #define D_0_52 (52) 105 | #define D_0_96 (96) 106 | #define D_0_104 (104) 107 | #define D_0_108 (108) 108 | #define D_0_163 (163) 109 | #define D_0_188 (188) 110 | #define D_0_192 (192) 111 | #define D_0_224 (224) 112 | #define D_0_228 (228) 113 | #define D_0_232 (232) 114 | #define D_0_236 (236) 115 | 116 | #define D_1_2 (256 + 2) 117 | #define D_1_4 (256 + 4) 118 | #define D_1_8 (256 + 8) 119 | #define D_1_10 (256 + 10) 120 | #define D_1_24 (256 + 24) 121 | #define D_1_28 (256 + 28) 122 | #define D_1_36 (256 + 36) 123 | #define D_1_40 (256 + 40) 124 | #define D_1_44 (256 + 44) 125 | #define D_1_72 (256 + 72) 126 | #define D_1_74 (256 + 74) 127 | #define D_1_79 (256 + 79) 128 | #define D_1_88 (256 + 88) 129 | #define D_1_90 (256 + 90) 130 | #define D_1_92 (256 + 92) 131 | #define D_1_96 (256 + 96) 132 | #define D_1_98 (256 + 98) 133 | #define D_1_106 (256 + 106) 134 | #define D_1_108 (256 + 108) 135 | #define D_1_112 (256 + 112) 136 | #define D_1_128 (256 + 144) 137 | #define D_1_152 (256 + 12) 138 | #define D_1_160 (256 + 160) 139 | #define D_1_176 (256 + 176) 140 | #define D_1_178 (256 + 178) 141 | #define D_1_218 (256 + 218) 142 | #define D_1_232 (256 + 232) 143 | #define D_1_236 (256 + 236) 144 | #define D_1_240 (256 + 240) 145 | #define D_1_244 (256 + 244) 146 | #define D_1_250 (256 + 250) 147 | #define D_1_252 (256 + 252) 148 | #define D_2_12 (512 + 12) 149 | #define D_2_96 (512 + 96) 150 | #define D_2_108 (512 + 108) 151 | #define D_2_208 (512 + 208) 152 | #define D_2_224 (512 + 224) 153 | #define D_2_236 (512 + 236) 154 | #define D_2_244 (512 + 244) 155 | #define D_2_248 (512 + 248) 156 | #define D_2_252 (512 + 252) 157 | 158 | #define CPASS_BIAS_X (35 * 16 + 4) 159 | #define CPASS_BIAS_Y (35 * 16 + 8) 160 | #define CPASS_BIAS_Z (35 * 16 + 12) 161 | #define CPASS_MTX_00 (36 * 16) 162 | #define CPASS_MTX_01 (36 * 16 + 4) 163 | #define CPASS_MTX_02 (36 * 16 + 8) 164 | #define CPASS_MTX_10 (36 * 16 + 12) 165 | #define CPASS_MTX_11 (37 * 16) 166 | #define CPASS_MTX_12 (37 * 16 + 4) 167 | #define CPASS_MTX_20 (37 * 16 + 8) 168 | #define CPASS_MTX_21 (37 * 16 + 12) 169 | #define CPASS_MTX_22 (43 * 16 + 12) 170 | #define D_EXT_GYRO_BIAS_X (61 * 16) 171 | #define D_EXT_GYRO_BIAS_Y (61 * 16) + 4 172 | #define D_EXT_GYRO_BIAS_Z (61 * 16) + 8 173 | #define D_ACT0 (40 * 16) 174 | #define D_ACSX (40 * 16 + 4) 175 | #define D_ACSY (40 * 16 + 8) 176 | #define D_ACSZ (40 * 16 + 12) 177 | 178 | #define FLICK_MSG (45 * 16 + 4) 179 | #define FLICK_COUNTER (45 * 16 + 8) 180 | #define FLICK_LOWER (45 * 16 + 12) 181 | #define FLICK_UPPER (46 * 16 + 12) 182 | 183 | #define D_AUTH_OUT (992) 184 | #define D_AUTH_IN (996) 185 | #define D_AUTH_A (1000) 186 | #define D_AUTH_B (1004) 187 | 188 | #define D_PEDSTD_BP_B (768 + 0x1C) 189 | #define D_PEDSTD_HP_A (768 + 0x78) 190 | #define D_PEDSTD_HP_B (768 + 0x7C) 191 | #define D_PEDSTD_BP_A4 (768 + 0x40) 192 | #define D_PEDSTD_BP_A3 (768 + 0x44) 193 | #define D_PEDSTD_BP_A2 (768 + 0x48) 194 | #define D_PEDSTD_BP_A1 (768 + 0x4C) 195 | #define D_PEDSTD_INT_THRSH (768 + 0x68) 196 | #define D_PEDSTD_CLIP (768 + 0x6C) 197 | #define D_PEDSTD_SB (768 + 0x28) 198 | #define D_PEDSTD_SB_TIME (768 + 0x2C) 199 | #define D_PEDSTD_PEAKTHRSH (768 + 0x98) 200 | #define D_PEDSTD_TIML (768 + 0x2A) 201 | #define D_PEDSTD_TIMH (768 + 0x2E) 202 | #define D_PEDSTD_PEAK (768 + 0X94) 203 | #define D_PEDSTD_STEPCTR (768 + 0x60) 204 | #define D_PEDSTD_TIMECTR (964) 205 | #define D_PEDSTD_DECI (768 + 0xA0) 206 | 207 | #define D_HOST_NO_MOT (976) 208 | #define D_ACCEL_BIAS (660) 209 | 210 | #define D_ORIENT_GAP (76) 211 | 212 | #define D_TILT0_H (48) 213 | #define D_TILT0_L (50) 214 | #define D_TILT1_H (52) 215 | #define D_TILT1_L (54) 216 | #define D_TILT2_H (56) 217 | #define D_TILT2_L (58) 218 | #define D_TILT3_H (60) 219 | #define D_TILT3_L (62) 220 | 221 | #define DMP_CODE_SIZE (3062) 222 | 223 | static const unsigned char dmp_memory[DMP_CODE_SIZE] = { 224 | /* bank # 0 */ 225 | 0x00, 0x00, 0x70, 0x00, 0x00, 0x00, 0x00, 0x24, 0x00, 0x00, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00, 226 | 0x00, 0x65, 0x00, 0x54, 0xff, 0xef, 0x00, 0x00, 0xfa, 0x80, 0x00, 0x0b, 0x12, 0x82, 0x00, 0x01, 227 | 0x03, 0x0c, 0x30, 0xc3, 0x0e, 0x8c, 0x8c, 0xe9, 0x14, 0xd5, 0x40, 0x02, 0x13, 0x71, 0x0f, 0x8e, 228 | 0x38, 0x83, 0xf8, 0x83, 0x30, 0x00, 0xf8, 0x83, 0x25, 0x8e, 0xf8, 0x83, 0x30, 0x00, 0xf8, 0x83, 229 | 0xff, 0xff, 0xff, 0xff, 0x0f, 0xfe, 0xa9, 0xd6, 0x24, 0x00, 0x04, 0x00, 0x1a, 0x82, 0x79, 0xa1, 230 | 0x00, 0x00, 0x00, 0x3c, 0xff, 0xff, 0x00, 0x00, 0x00, 0x10, 0x00, 0x00, 0x38, 0x83, 0x6f, 0xa2, 231 | 0x00, 0x3e, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xca, 0xe3, 0x09, 0x3e, 0x80, 0x00, 0x00, 232 | 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00, 233 | 0x00, 0x0c, 0x00, 0x00, 0x00, 0x0c, 0x18, 0x6e, 0x00, 0x00, 0x06, 0x92, 0x0a, 0x16, 0xc0, 0xdf, 234 | 0xff, 0xff, 0x02, 0x56, 0xfd, 0x8c, 0xd3, 0x77, 0xff, 0xe1, 0xc4, 0x96, 0xe0, 0xc5, 0xbe, 0xaa, 235 | 0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0x0b, 0x2b, 0x00, 0x00, 0x16, 0x57, 0x00, 0x00, 0x03, 0x59, 236 | 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1d, 0xfa, 0x00, 0x02, 0x6c, 0x1d, 0x00, 0x00, 0x00, 0x00, 237 | 0x3f, 0xff, 0xdf, 0xeb, 0x00, 0x3e, 0xb3, 0xb6, 0x00, 0x0d, 0x22, 0x78, 0x00, 0x00, 0x2f, 0x3c, 238 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x19, 0x42, 0xb5, 0x00, 0x00, 0x39, 0xa2, 0x00, 0x00, 0xb3, 0x65, 239 | 0xd9, 0x0e, 0x9f, 0xc9, 0x1d, 0xcf, 0x4c, 0x34, 0x30, 0x00, 0x00, 0x00, 0x50, 0x00, 0x00, 0x00, 240 | 0x3b, 0xb6, 0x7a, 0xe8, 0x00, 0x64, 0x00, 0x00, 0x00, 0xc8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 241 | /* bank # 1 */ 242 | 0x10, 0x00, 0x00, 0x00, 0x10, 0x00, 0xfa, 0x92, 0x10, 0x00, 0x22, 0x5e, 0x00, 0x0d, 0x22, 0x9f, 243 | 0x00, 0x01, 0x00, 0x00, 0x00, 0x32, 0x00, 0x00, 0xff, 0x46, 0x00, 0x00, 0x63, 0xd4, 0x00, 0x00, 244 | 0x10, 0x00, 0x00, 0x00, 0x04, 0xd6, 0x00, 0x00, 0x04, 0xcc, 0x00, 0x00, 0x04, 0xcc, 0x00, 0x00, 245 | 0x00, 0x00, 0x10, 0x72, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 246 | 0x00, 0x06, 0x00, 0x02, 0x00, 0x05, 0x00, 0x07, 0x00, 0x00, 0x00, 0x00, 0x00, 0x64, 0x00, 0x00, 247 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x05, 0x00, 0x05, 0x00, 0x64, 0x00, 0x20, 0x00, 0x00, 248 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x03, 0x00, 249 | 0x00, 0x00, 0x00, 0x32, 0xf8, 0x98, 0x00, 0x00, 0xff, 0x65, 0x00, 0x00, 0x83, 0x0f, 0x00, 0x00, 250 | 0xff, 0x9b, 0xfc, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 251 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 252 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x00, 253 | 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0xb2, 0x6a, 0x00, 0x02, 0x00, 0x00, 254 | 0x00, 0x01, 0xfb, 0x83, 0x00, 0x68, 0x00, 0x00, 0x00, 0xd9, 0xfc, 0x00, 0x7c, 0xf1, 0xff, 0x83, 255 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x64, 0x03, 0xe8, 0x00, 0x64, 0x00, 0x28, 256 | 0x00, 0x00, 0x00, 0x25, 0x00, 0x00, 0x00, 0x00, 0x16, 0xa0, 0x00, 0x00, 0x00, 0x00, 0x10, 0x00, 257 | 0x00, 0x00, 0x10, 0x00, 0x00, 0x2f, 0x00, 0x00, 0x00, 0x00, 0x01, 0xf4, 0x00, 0x00, 0x10, 0x00, 258 | /* bank # 2 */ 259 | 0x00, 0x28, 0x00, 0x00, 0xff, 0xff, 0x45, 0x81, 0xff, 0xff, 0xfa, 0x72, 0x00, 0x00, 0x00, 0x00, 260 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x44, 0x00, 0x05, 0x00, 0x05, 0xba, 0xc6, 0x00, 0x47, 0x78, 0xa2, 261 | 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14, 262 | 0x00, 0x00, 0x25, 0x4d, 0x00, 0x2f, 0x70, 0x6d, 0x00, 0x00, 0x05, 0xae, 0x00, 0x0c, 0x02, 0xd0, 263 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 264 | 0x00, 0x1b, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 265 | 0x00, 0x64, 0x00, 0x00, 0x00, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 266 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 267 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 268 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 269 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 270 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 271 | 0x00, 0x1b, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0e, 0x00, 0x0e, 272 | 0x00, 0x00, 0x0a, 0xc7, 0x00, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x32, 0xff, 0xff, 0xff, 0x9c, 273 | 0x00, 0x00, 0x0b, 0x2b, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x64, 274 | 0xff, 0xe5, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 275 | /* bank # 3 */ 276 | 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 277 | 0x00, 0x01, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0x24, 0x26, 0xd3, 278 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x10, 0x00, 0x96, 0x00, 0x3c, 279 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 280 | 0x0c, 0x0a, 0x4e, 0x68, 0xcd, 0xcf, 0x77, 0x09, 0x50, 0x16, 0x67, 0x59, 0xc6, 0x19, 0xce, 0x82, 281 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 282 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x17, 0xd7, 0x84, 0x00, 0x03, 0x00, 0x00, 0x00, 283 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xc7, 0x93, 0x8f, 0x9d, 0x1e, 0x1b, 0x1c, 0x19, 284 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 285 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x03, 0x18, 0x85, 0x00, 0x00, 0x40, 0x00, 286 | 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 287 | 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 288 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 289 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 290 | 0x00, 0x00, 0x00, 0x00, 0x67, 0x7d, 0xdf, 0x7e, 0x72, 0x90, 0x2e, 0x55, 0x4c, 0xf6, 0xe6, 0x88, 291 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 292 | 293 | /* bank # 4 */ 294 | 0xd8, 0xdc, 0xb4, 0xb8, 0xb0, 0xd8, 0xb9, 0xab, 0xf3, 0xf8, 0xfa, 0xb3, 0xb7, 0xbb, 0x8e, 0x9e, 295 | 0xae, 0xf1, 0x32, 0xf5, 0x1b, 0xf1, 0xb4, 0xb8, 0xb0, 0x80, 0x97, 0xf1, 0xa9, 0xdf, 0xdf, 0xdf, 296 | 0xaa, 0xdf, 0xdf, 0xdf, 0xf2, 0xaa, 0xc5, 0xcd, 0xc7, 0xa9, 0x0c, 0xc9, 0x2c, 0x97, 0xf1, 0xa9, 297 | 0x89, 0x26, 0x46, 0x66, 0xb2, 0x89, 0x99, 0xa9, 0x2d, 0x55, 0x7d, 0xb0, 0xb0, 0x8a, 0xa8, 0x96, 298 | 0x36, 0x56, 0x76, 0xf1, 0xba, 0xa3, 0xb4, 0xb2, 0x80, 0xc0, 0xb8, 0xa8, 0x97, 0x11, 0xb2, 0x83, 299 | 0x98, 0xba, 0xa3, 0xf0, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xb2, 0xb9, 0xb4, 0x98, 0x83, 0xf1, 300 | 0xa3, 0x29, 0x55, 0x7d, 0xba, 0xb5, 0xb1, 0xa3, 0x83, 0x93, 0xf0, 0x00, 0x28, 0x50, 0xf5, 0xb2, 301 | 0xb6, 0xaa, 0x83, 0x93, 0x28, 0x54, 0x7c, 0xf1, 0xb9, 0xa3, 0x82, 0x93, 0x61, 0xba, 0xa2, 0xda, 302 | 0xde, 0xdf, 0xdb, 0x81, 0x9a, 0xb9, 0xae, 0xf5, 0x60, 0x68, 0x70, 0xf1, 0xda, 0xba, 0xa2, 0xdf, 303 | 0xd9, 0xba, 0xa2, 0xfa, 0xb9, 0xa3, 0x82, 0x92, 0xdb, 0x31, 0xba, 0xa2, 0xd9, 0xba, 0xa2, 0xf8, 304 | 0xdf, 0x85, 0xa4, 0xd0, 0xc1, 0xbb, 0xad, 0x83, 0xc2, 0xc5, 0xc7, 0xb8, 0xa2, 0xdf, 0xdf, 0xdf, 305 | 0xba, 0xa0, 0xdf, 0xdf, 0xdf, 0xd8, 0xd8, 0xf1, 0xb8, 0xaa, 0xb3, 0x8d, 0xb4, 0x98, 0x0d, 0x35, 306 | 0x5d, 0xb2, 0xb6, 0xba, 0xaf, 0x8c, 0x96, 0x19, 0x8f, 0x9f, 0xa7, 0x0e, 0x16, 0x1e, 0xb4, 0x9a, 307 | 0xb8, 0xaa, 0x87, 0x2c, 0x54, 0x7c, 0xba, 0xa4, 0xb0, 0x8a, 0xb6, 0x91, 0x32, 0x56, 0x76, 0xb2, 308 | 0x84, 0x94, 0xa4, 0xc8, 0x08, 0xcd, 0xd8, 0xb8, 0xb4, 0xb0, 0xf1, 0x99, 0x82, 0xa8, 0x2d, 0x55, 309 | 0x7d, 0x98, 0xa8, 0x0e, 0x16, 0x1e, 0xa2, 0x2c, 0x54, 0x7c, 0x92, 0xa4, 0xf0, 0x2c, 0x50, 0x78, 310 | /* bank # 5 */ 311 | 0xf1, 0x84, 0xa8, 0x98, 0xc4, 0xcd, 0xfc, 0xd8, 0x0d, 0xdb, 0xa8, 0xfc, 0x2d, 0xf3, 0xd9, 0xba, 312 | 0xa6, 0xf8, 0xda, 0xba, 0xa6, 0xde, 0xd8, 0xba, 0xb2, 0xb6, 0x86, 0x96, 0xa6, 0xd0, 0xf3, 0xc8, 313 | 0x41, 0xda, 0xa6, 0xc8, 0xf8, 0xd8, 0xb0, 0xb4, 0xb8, 0x82, 0xa8, 0x92, 0xf5, 0x2c, 0x54, 0x88, 314 | 0x98, 0xf1, 0x35, 0xd9, 0xf4, 0x18, 0xd8, 0xf1, 0xa2, 0xd0, 0xf8, 0xf9, 0xa8, 0x84, 0xd9, 0xc7, 315 | 0xdf, 0xf8, 0xf8, 0x83, 0xc5, 0xda, 0xdf, 0x69, 0xdf, 0x83, 0xc1, 0xd8, 0xf4, 0x01, 0x14, 0xf1, 316 | 0xa8, 0x82, 0x4e, 0xa8, 0x84, 0xf3, 0x11, 0xd1, 0x82, 0xf5, 0xd9, 0x92, 0x28, 0x97, 0x88, 0xf1, 317 | 0x09, 0xf4, 0x1c, 0x1c, 0xd8, 0x84, 0xa8, 0xf3, 0xc0, 0xf9, 0xd1, 0xd9, 0x97, 0x82, 0xf1, 0x29, 318 | 0xf4, 0x0d, 0xd8, 0xf3, 0xf9, 0xf9, 0xd1, 0xd9, 0x82, 0xf4, 0xc2, 0x03, 0xd8, 0xde, 0xdf, 0x1a, 319 | 0xd8, 0xf1, 0xa2, 0xfa, 0xf9, 0xa8, 0x84, 0x98, 0xd9, 0xc7, 0xdf, 0xf8, 0xf8, 0xf8, 0x83, 0xc7, 320 | 0xda, 0xdf, 0x69, 0xdf, 0xf8, 0x83, 0xc3, 0xd8, 0xf4, 0x01, 0x14, 0xf1, 0x98, 0xa8, 0x82, 0x2e, 321 | 0xa8, 0x84, 0xf3, 0x11, 0xd1, 0x82, 0xf5, 0xd9, 0x92, 0x50, 0x97, 0x88, 0xf1, 0x09, 0xf4, 0x1c, 322 | 0xd8, 0x84, 0xa8, 0xf3, 0xc0, 0xf8, 0xf9, 0xd1, 0xd9, 0x97, 0x82, 0xf1, 0x49, 0xf4, 0x0d, 0xd8, 323 | 0xf3, 0xf9, 0xf9, 0xd1, 0xd9, 0x82, 0xf4, 0xc4, 0x03, 0xd8, 0xde, 0xdf, 0xd8, 0xf1, 0xad, 0x88, 324 | 0x98, 0xcc, 0xa8, 0x09, 0xf9, 0xd9, 0x82, 0x92, 0xa8, 0xf5, 0x7c, 0xf1, 0x88, 0x3a, 0xcf, 0x94, 325 | 0x4a, 0x6e, 0x98, 0xdb, 0x69, 0x31, 0xda, 0xad, 0xf2, 0xde, 0xf9, 0xd8, 0x87, 0x95, 0xa8, 0xf2, 326 | 0x21, 0xd1, 0xda, 0xa5, 0xf9, 0xf4, 0x17, 0xd9, 0xf1, 0xae, 0x8e, 0xd0, 0xc0, 0xc3, 0xae, 0x82, 327 | /* bank # 6 */ 328 | 0xc6, 0x84, 0xc3, 0xa8, 0x85, 0x95, 0xc8, 0xa5, 0x88, 0xf2, 0xc0, 0xf1, 0xf4, 0x01, 0x0e, 0xf1, 329 | 0x8e, 0x9e, 0xa8, 0xc6, 0x3e, 0x56, 0xf5, 0x54, 0xf1, 0x88, 0x72, 0xf4, 0x01, 0x15, 0xf1, 0x98, 330 | 0x45, 0x85, 0x6e, 0xf5, 0x8e, 0x9e, 0x04, 0x88, 0xf1, 0x42, 0x98, 0x5a, 0x8e, 0x9e, 0x06, 0x88, 331 | 0x69, 0xf4, 0x01, 0x1c, 0xf1, 0x98, 0x1e, 0x11, 0x08, 0xd0, 0xf5, 0x04, 0xf1, 0x1e, 0x97, 0x02, 332 | 0x02, 0x98, 0x36, 0x25, 0xdb, 0xf9, 0xd9, 0x85, 0xa5, 0xf3, 0xc1, 0xda, 0x85, 0xa5, 0xf3, 0xdf, 333 | 0xd8, 0x85, 0x95, 0xa8, 0xf3, 0x09, 0xda, 0xa5, 0xfa, 0xd8, 0x82, 0x92, 0xa8, 0xf5, 0x78, 0xf1, 334 | 0x88, 0x1a, 0x84, 0x9f, 0x26, 0x88, 0x98, 0x21, 0xda, 0xf4, 0x1d, 0xf3, 0xd8, 0x87, 0x9f, 0x39, 335 | 0xd1, 0xaf, 0xd9, 0xdf, 0xdf, 0xfb, 0xf9, 0xf4, 0x0c, 0xf3, 0xd8, 0xfa, 0xd0, 0xf8, 0xda, 0xf9, 336 | 0xf9, 0xd0, 0xdf, 0xd9, 0xf9, 0xd8, 0xf4, 0x0b, 0xd8, 0xf3, 0x87, 0x9f, 0x39, 0xd1, 0xaf, 0xd9, 337 | 0xdf, 0xdf, 0xf4, 0x1d, 0xf3, 0xd8, 0xfa, 0xfc, 0xa8, 0x69, 0xf9, 0xf9, 0xaf, 0xd0, 0xda, 0xde, 338 | 0xfa, 0xd9, 0xf8, 0x8f, 0x9f, 0xa8, 0xf1, 0xcc, 0xf3, 0x98, 0xdb, 0x45, 0xd9, 0xaf, 0xdf, 0xd0, 339 | 0xf8, 0xd8, 0xf1, 0x8f, 0x9f, 0xa8, 0xca, 0xf3, 0x88, 0x09, 0xda, 0xaf, 0x8f, 0xcb, 0xf8, 0xd8, 340 | 0xf2, 0xad, 0x97, 0x8d, 0x0c, 0xd9, 0xa5, 0xdf, 0xf9, 0xba, 0xa6, 0xf3, 0xfa, 0xf4, 0x12, 0xf2, 341 | 0xd8, 0x95, 0x0d, 0xd1, 0xd9, 0xba, 0xa6, 0xf3, 0xfa, 0xda, 0xa5, 0xf2, 0xc1, 0xba, 0xa6, 0xf3, 342 | 0xdf, 0xd8, 0xf1, 0xba, 0xb2, 0xb6, 0x86, 0x96, 0xa6, 0xd0, 0xca, 0xf3, 0x49, 0xda, 0xa6, 0xcb, 343 | 0xf8, 0xd8, 0xb0, 0xb4, 0xb8, 0xd8, 0xad, 0x84, 0xf2, 0xc0, 0xdf, 0xf1, 0x8f, 0xcb, 0xc3, 0xa8, 344 | /* bank # 7 */ 345 | 0xb2, 0xb6, 0x86, 0x96, 0xc8, 0xc1, 0xcb, 0xc3, 0xf3, 0xb0, 0xb4, 0x88, 0x98, 0xa8, 0x21, 0xdb, 346 | 0x71, 0x8d, 0x9d, 0x71, 0x85, 0x95, 0x21, 0xd9, 0xad, 0xf2, 0xfa, 0xd8, 0x85, 0x97, 0xa8, 0x28, 347 | 0xd9, 0xf4, 0x08, 0xd8, 0xf2, 0x8d, 0x29, 0xda, 0xf4, 0x05, 0xd9, 0xf2, 0x85, 0xa4, 0xc2, 0xf2, 348 | 0xd8, 0xa8, 0x8d, 0x94, 0x01, 0xd1, 0xd9, 0xf4, 0x11, 0xf2, 0xd8, 0x87, 0x21, 0xd8, 0xf4, 0x0a, 349 | 0xd8, 0xf2, 0x84, 0x98, 0xa8, 0xc8, 0x01, 0xd1, 0xd9, 0xf4, 0x11, 0xd8, 0xf3, 0xa4, 0xc8, 0xbb, 350 | 0xaf, 0xd0, 0xf2, 0xde, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xd8, 0xf1, 0xb8, 0xf6, 351 | 0xb5, 0xb9, 0xb0, 0x8a, 0x95, 0xa3, 0xde, 0x3c, 0xa3, 0xd9, 0xf8, 0xd8, 0x5c, 0xa3, 0xd9, 0xf8, 352 | 0xd8, 0x7c, 0xa3, 0xd9, 0xf8, 0xd8, 0xf8, 0xf9, 0xd1, 0xa5, 0xd9, 0xdf, 0xda, 0xfa, 0xd8, 0xb1, 353 | 0x85, 0x30, 0xf7, 0xd9, 0xde, 0xd8, 0xf8, 0x30, 0xad, 0xda, 0xde, 0xd8, 0xf2, 0xb4, 0x8c, 0x99, 354 | 0xa3, 0x2d, 0x55, 0x7d, 0xa0, 0x83, 0xdf, 0xdf, 0xdf, 0xb5, 0x91, 0xa0, 0xf6, 0x29, 0xd9, 0xfb, 355 | 0xd8, 0xa0, 0xfc, 0x29, 0xd9, 0xfa, 0xd8, 0xa0, 0xd0, 0x51, 0xd9, 0xf8, 0xd8, 0xfc, 0x51, 0xd9, 356 | 0xf9, 0xd8, 0x79, 0xd9, 0xfb, 0xd8, 0xa0, 0xd0, 0xfc, 0x79, 0xd9, 0xfa, 0xd8, 0xa1, 0xf9, 0xf9, 357 | 0xf9, 0xf9, 0xf9, 0xa0, 0xda, 0xdf, 0xdf, 0xdf, 0xd8, 0xa1, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xac, 358 | 0xde, 0xf8, 0xad, 0xde, 0x83, 0x93, 0xac, 0x2c, 0x54, 0x7c, 0xf1, 0xa8, 0xdf, 0xdf, 0xdf, 0xf6, 359 | 0x9d, 0x2c, 0xda, 0xa0, 0xdf, 0xd9, 0xfa, 0xdb, 0x2d, 0xf8, 0xd8, 0xa8, 0x50, 0xda, 0xa0, 0xd0, 360 | 0xde, 0xd9, 0xd0, 0xf8, 0xf8, 0xf8, 0xdb, 0x55, 0xf8, 0xd8, 0xa8, 0x78, 0xda, 0xa0, 0xd0, 0xdf, 361 | /* bank # 8 */ 362 | 0xd9, 0xd0, 0xfa, 0xf8, 0xf8, 0xf8, 0xf8, 0xdb, 0x7d, 0xf8, 0xd8, 0x9c, 0xa8, 0x8c, 0xf5, 0x30, 363 | 0xdb, 0x38, 0xd9, 0xd0, 0xde, 0xdf, 0xa0, 0xd0, 0xde, 0xdf, 0xd8, 0xa8, 0x48, 0xdb, 0x58, 0xd9, 364 | 0xdf, 0xd0, 0xde, 0xa0, 0xdf, 0xd0, 0xde, 0xd8, 0xa8, 0x68, 0xdb, 0x70, 0xd9, 0xdf, 0xdf, 0xa0, 365 | 0xdf, 0xdf, 0xd8, 0xf1, 0xa8, 0x88, 0x90, 0x2c, 0x54, 0x7c, 0x98, 0xa8, 0xd0, 0x5c, 0x38, 0xd1, 366 | 0xda, 0xf2, 0xae, 0x8c, 0xdf, 0xf9, 0xd8, 0xb0, 0x87, 0xa8, 0xc1, 0xc1, 0xb1, 0x88, 0xa8, 0xc6, 367 | 0xf9, 0xf9, 0xda, 0x36, 0xd8, 0xa8, 0xf9, 0xda, 0x36, 0xd8, 0xa8, 0xf9, 0xda, 0x36, 0xd8, 0xa8, 368 | 0xf9, 0xda, 0x36, 0xd8, 0xa8, 0xf9, 0xda, 0x36, 0xd8, 0xf7, 0x8d, 0x9d, 0xad, 0xf8, 0x18, 0xda, 369 | 0xf2, 0xae, 0xdf, 0xd8, 0xf7, 0xad, 0xfa, 0x30, 0xd9, 0xa4, 0xde, 0xf9, 0xd8, 0xf2, 0xae, 0xde, 370 | 0xfa, 0xf9, 0x83, 0xa7, 0xd9, 0xc3, 0xc5, 0xc7, 0xf1, 0x88, 0x9b, 0xa7, 0x7a, 0xad, 0xf7, 0xde, 371 | 0xdf, 0xa4, 0xf8, 0x84, 0x94, 0x08, 0xa7, 0x97, 0xf3, 0x00, 0xae, 0xf2, 0x98, 0x19, 0xa4, 0x88, 372 | 0xc6, 0xa3, 0x94, 0x88, 0xf6, 0x32, 0xdf, 0xf2, 0x83, 0x93, 0xdb, 0x09, 0xd9, 0xf2, 0xaa, 0xdf, 373 | 0xd8, 0xd8, 0xae, 0xf8, 0xf9, 0xd1, 0xda, 0xf3, 0xa4, 0xde, 0xa7, 0xf1, 0x88, 0x9b, 0x7a, 0xd8, 374 | 0xf3, 0x84, 0x94, 0xae, 0x19, 0xf9, 0xda, 0xaa, 0xf1, 0xdf, 0xd8, 0xa8, 0x81, 0xc0, 0xc3, 0xc5, 375 | 0xc7, 0xa3, 0x92, 0x83, 0xf6, 0x28, 0xad, 0xde, 0xd9, 0xf8, 0xd8, 0xa3, 0x50, 0xad, 0xd9, 0xf8, 376 | 0xd8, 0xa3, 0x78, 0xad, 0xd9, 0xf8, 0xd8, 0xf8, 0xf9, 0xd1, 0xa1, 0xda, 0xde, 0xc3, 0xc5, 0xc7, 377 | 0xd8, 0xa1, 0x81, 0x94, 0xf8, 0x18, 0xf2, 0xb0, 0x89, 0xac, 0xc3, 0xc5, 0xc7, 0xf1, 0xd8, 0xb8, 378 | /* bank # 9 */ 379 | 0xb4, 0xb0, 0x97, 0x86, 0xa8, 0x31, 0x9b, 0x06, 0x99, 0x07, 0xab, 0x97, 0x28, 0x88, 0x9b, 0xf0, 380 | 0x0c, 0x20, 0x14, 0x40, 0xb0, 0xb4, 0xb8, 0xf0, 0xa8, 0x8a, 0x9a, 0x28, 0x50, 0x78, 0xb7, 0x9b, 381 | 0xa8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31, 0xf1, 0xbb, 0xab, 382 | 0x88, 0x00, 0x2c, 0x54, 0x7c, 0xf0, 0xb3, 0x8b, 0xb8, 0xa8, 0x04, 0x28, 0x50, 0x78, 0xf1, 0xb0, 383 | 0x88, 0xb4, 0x97, 0x26, 0xa8, 0x59, 0x98, 0xbb, 0xab, 0xb3, 0x8b, 0x02, 0x26, 0x46, 0x66, 0xb0, 384 | 0xb8, 0xf0, 0x8a, 0x9c, 0xa8, 0x29, 0x51, 0x79, 0x8b, 0x29, 0x51, 0x79, 0x8a, 0x24, 0x70, 0x59, 385 | 0x8b, 0x20, 0x58, 0x71, 0x8a, 0x44, 0x69, 0x38, 0x8b, 0x39, 0x40, 0x68, 0x8a, 0x64, 0x48, 0x31, 386 | 0x8b, 0x30, 0x49, 0x60, 0x88, 0xf1, 0xac, 0x00, 0x2c, 0x54, 0x7c, 0xf0, 0x8c, 0xa8, 0x04, 0x28, 387 | 0x50, 0x78, 0xf1, 0x88, 0x97, 0x26, 0xa8, 0x59, 0x98, 0xac, 0x8c, 0x02, 0x26, 0x46, 0x66, 0xf0, 388 | 0x89, 0x9c, 0xa8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31, 0xa9, 389 | 0x88, 0x09, 0x20, 0x59, 0x70, 0xab, 0x11, 0x38, 0x40, 0x69, 0xa8, 0x19, 0x31, 0x48, 0x60, 0x8c, 390 | 0xa8, 0x3c, 0x41, 0x5c, 0x20, 0x7c, 0x00, 0xf1, 0x87, 0x98, 0x19, 0x86, 0xa8, 0x6e, 0x76, 0x7e, 391 | 0xa9, 0x99, 0x88, 0x2d, 0x55, 0x7d, 0xd8, 0xb1, 0xb5, 0xb9, 0xa3, 0xdf, 0xdf, 0xdf, 0xae, 0xd0, 392 | 0xdf, 0xaa, 0xd0, 0xde, 0xf2, 0xab, 0xf8, 0xf9, 0xd9, 0xb0, 0x87, 0xc4, 0xaa, 0xf1, 0xdf, 0xdf, 393 | 0xbb, 0xaf, 0xdf, 0xdf, 0xb9, 0xd8, 0xb1, 0xf1, 0xa3, 0x97, 0x8e, 0x60, 0xdf, 0xb0, 0x84, 0xf2, 394 | 0xc8, 0xf8, 0xf9, 0xd9, 0xde, 0xd8, 0x93, 0x85, 0xf1, 0x4a, 0xb1, 0x83, 0xa3, 0x08, 0xb5, 0x83, 395 | /* bank # 10 */ 396 | 0x9a, 0x08, 0x10, 0xb7, 0x9f, 0x10, 0xd8, 0xf1, 0xb0, 0xba, 0xae, 0xb0, 0x8a, 0xc2, 0xb2, 0xb6, 397 | 0x8e, 0x9e, 0xf1, 0xfb, 0xd9, 0xf4, 0x1d, 0xd8, 0xf9, 0xd9, 0x0c, 0xf1, 0xd8, 0xf8, 0xf8, 0xad, 398 | 0x61, 0xd9, 0xae, 0xfb, 0xd8, 0xf4, 0x0c, 0xf1, 0xd8, 0xf8, 0xf8, 0xad, 0x19, 0xd9, 0xae, 0xfb, 399 | 0xdf, 0xd8, 0xf4, 0x16, 0xf1, 0xd8, 0xf8, 0xad, 0x8d, 0x61, 0xd9, 0xf4, 0xf4, 0xac, 0xf5, 0x9c, 400 | 0x9c, 0x8d, 0xdf, 0x2b, 0xba, 0xb6, 0xae, 0xfa, 0xf8, 0xf4, 0x0b, 0xd8, 0xf1, 0xae, 0xd0, 0xf8, 401 | 0xad, 0x51, 0xda, 0xae, 0xfa, 0xf8, 0xf1, 0xd8, 0xb9, 0xb1, 0xb6, 0xa3, 0x83, 0x9c, 0x08, 0xb9, 402 | 0xb1, 0x83, 0x9a, 0xb5, 0xaa, 0xc0, 0xfd, 0x30, 0x83, 0xb7, 0x9f, 0x10, 0xb5, 0x8b, 0x93, 0xf2, 403 | 0x02, 0x02, 0xd1, 0xab, 0xda, 0xde, 0xd8, 0xf1, 0xb0, 0x80, 0xba, 0xab, 0xc0, 0xc3, 0xb2, 0x84, 404 | 0xc1, 0xc3, 0xd8, 0xb1, 0xb9, 0xf3, 0x8b, 0xa3, 0x91, 0xb6, 0x09, 0xb4, 0xd9, 0xab, 0xde, 0xb0, 405 | 0x87, 0x9c, 0xb9, 0xa3, 0xdd, 0xf1, 0xb3, 0x8b, 0x8b, 0x8b, 0x8b, 0x8b, 0xb0, 0x87, 0xa3, 0xa3, 406 | 0xa3, 0xa3, 0xb2, 0x8b, 0xb6, 0x9b, 0xf2, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 407 | 0xa3, 0xf1, 0xb0, 0x87, 0xb5, 0x9a, 0xa3, 0xf3, 0x9b, 0xa3, 0xa3, 0xdc, 0xba, 0xac, 0xdf, 0xb9, 408 | 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 409 | 0xd8, 0xd8, 0xd8, 0xbb, 0xb3, 0xb7, 0xf1, 0xaa, 0xf9, 0xda, 0xff, 0xd9, 0x80, 0x9a, 0xaa, 0x28, 410 | 0xb4, 0x80, 0x98, 0xa7, 0x20, 0xb7, 0x97, 0x87, 0xa8, 0x66, 0x88, 0xf0, 0x79, 0x51, 0xf1, 0x90, 411 | 0x2c, 0x87, 0x0c, 0xa7, 0x81, 0x97, 0x62, 0x93, 0xf0, 0x71, 0x71, 0x60, 0x85, 0x94, 0x01, 0x29, 412 | /* bank # 11 */ 413 | 0x51, 0x79, 0x90, 0xa5, 0xf1, 0x28, 0x4c, 0x6c, 0x87, 0x0c, 0x95, 0x18, 0x85, 0x78, 0xa3, 0x83, 414 | 0x90, 0x28, 0x4c, 0x6c, 0x88, 0x6c, 0xd8, 0xf3, 0xa2, 0x82, 0x00, 0xf2, 0x10, 0xa8, 0x92, 0x19, 415 | 0x80, 0xa2, 0xf2, 0xd9, 0x26, 0xd8, 0xf1, 0x88, 0xa8, 0x4d, 0xd9, 0x48, 0xd8, 0x96, 0xa8, 0x39, 416 | 0x80, 0xd9, 0x3c, 0xd8, 0x95, 0x80, 0xa8, 0x39, 0xa6, 0x86, 0x98, 0xd9, 0x2c, 0xda, 0x87, 0xa7, 417 | 0x2c, 0xd8, 0xa8, 0x89, 0x95, 0x19, 0xa9, 0x80, 0xd9, 0x38, 0xd8, 0xa8, 0x89, 0x39, 0xa9, 0x80, 418 | 0xda, 0x3c, 0xd8, 0xa8, 0x2e, 0xa8, 0x39, 0x90, 0xd9, 0x0c, 0xd8, 0xa8, 0x95, 0x31, 0x98, 0xd9, 419 | 0x0c, 0xd8, 0xa8, 0x09, 0xd9, 0xff, 0xd8, 0x01, 0xda, 0xff, 0xd8, 0x95, 0x39, 0xa9, 0xda, 0x26, 420 | 0xff, 0xd8, 0x90, 0xa8, 0x0d, 0x89, 0x99, 0xa8, 0x10, 0x80, 0x98, 0x21, 0xda, 0x2e, 0xd8, 0x89, 421 | 0x99, 0xa8, 0x31, 0x80, 0xda, 0x2e, 0xd8, 0xa8, 0x86, 0x96, 0x31, 0x80, 0xda, 0x2e, 0xd8, 0xa8, 422 | 0x87, 0x31, 0x80, 0xda, 0x2e, 0xd8, 0xa8, 0x82, 0x92, 0xf3, 0x41, 0x80, 0xf1, 0xd9, 0x2e, 0xd8, 423 | 0xa8, 0x82, 0xf3, 0x19, 0x80, 0xf1, 0xd9, 0x2e, 0xd8, 0x82, 0xac, 0xf3, 0xc0, 0xa2, 0x80, 0x22, 424 | 0xf1, 0xa6, 0x2e, 0xa7, 0x2e, 0xa9, 0x22, 0x98, 0xa8, 0x29, 0xda, 0xac, 0xde, 0xff, 0xd8, 0xa2, 425 | 0xf2, 0x2a, 0xf1, 0xa9, 0x2e, 0x82, 0x92, 0xa8, 0xf2, 0x31, 0x80, 0xa6, 0x96, 0xf1, 0xd9, 0x00, 426 | 0xac, 0x8c, 0x9c, 0x0c, 0x30, 0xac, 0xde, 0xd0, 0xde, 0xff, 0xd8, 0x8c, 0x9c, 0xac, 0xd0, 0x10, 427 | 0xac, 0xde, 0x80, 0x92, 0xa2, 0xf2, 0x4c, 0x82, 0xa8, 0xf1, 0xca, 0xf2, 0x35, 0xf1, 0x96, 0x88, 428 | 0xa6, 0xd9, 0x00, 0xd8, 0xf1, 0xff 429 | }; 430 | 431 | static const unsigned short sStartAddress = 0x0400; 432 | 433 | /* END OF SECTION COPIED FROM dmpDefaultMPU6050.c */ 434 | 435 | #define INT_SRC_TAP (0x01) 436 | #define INT_SRC_ANDROID_ORIENT (0x08) 437 | 438 | #define DMP_FEATURE_SEND_ANY_GYRO (DMP_FEATURE_SEND_RAW_GYRO | \ 439 | DMP_FEATURE_SEND_CAL_GYRO) 440 | 441 | #define MAX_PACKET_LENGTH (32) 442 | 443 | #define DMP_SAMPLE_RATE (200) 444 | #define GYRO_SF (46850825LL * 200 / DMP_SAMPLE_RATE) 445 | 446 | #define FIFO_CORRUPTION_CHECK 447 | #ifdef FIFO_CORRUPTION_CHECK 448 | #define QUAT_ERROR_THRESH (1L<<24) 449 | #define QUAT_MAG_SQ_NORMALIZED (1L<<28) 450 | #define QUAT_MAG_SQ_MIN (QUAT_MAG_SQ_NORMALIZED - QUAT_ERROR_THRESH) 451 | #define QUAT_MAG_SQ_MAX (QUAT_MAG_SQ_NORMALIZED + QUAT_ERROR_THRESH) 452 | #endif 453 | 454 | struct dmp_s { 455 | void (*tap_cb)(unsigned char count, unsigned char direction); 456 | void (*android_orient_cb)(unsigned char orientation); 457 | unsigned short orient; 458 | unsigned short feature_mask; 459 | unsigned short fifo_rate; 460 | unsigned char packet_length; 461 | }; 462 | 463 | static struct dmp_s dmp = { 464 | .tap_cb = NULL, 465 | .android_orient_cb = NULL, 466 | .orient = 0, 467 | .feature_mask = 0, 468 | .fifo_rate = 0, 469 | .packet_length = 0 470 | }; 471 | 472 | /** 473 | * @brief Load the DMP with this image. 474 | * @return 0 if successful. 475 | */ 476 | int dmp_load_motion_driver_firmware(void) 477 | { 478 | return mpu_load_firmware(DMP_CODE_SIZE, dmp_memory, sStartAddress, 479 | DMP_SAMPLE_RATE); 480 | } 481 | 482 | /** 483 | * @brief Push gyro and accel orientation to the DMP. 484 | * The orientation is represented here as the output of 485 | * @e inv_orientation_matrix_to_scalar. 486 | * @param[in] orient Gyro and accel orientation in body frame. 487 | * @return 0 if successful. 488 | */ 489 | int dmp_set_orientation(unsigned short orient) 490 | { 491 | unsigned char gyro_regs[3], accel_regs[3]; 492 | const unsigned char gyro_axes[3] = {DINA4C, DINACD, DINA6C}; 493 | const unsigned char accel_axes[3] = {DINA0C, DINAC9, DINA2C}; 494 | const unsigned char gyro_sign[3] = {DINA36, DINA56, DINA76}; 495 | const unsigned char accel_sign[3] = {DINA26, DINA46, DINA66}; 496 | 497 | gyro_regs[0] = gyro_axes[orient & 3]; 498 | gyro_regs[1] = gyro_axes[(orient >> 3) & 3]; 499 | gyro_regs[2] = gyro_axes[(orient >> 6) & 3]; 500 | accel_regs[0] = accel_axes[orient & 3]; 501 | accel_regs[1] = accel_axes[(orient >> 3) & 3]; 502 | accel_regs[2] = accel_axes[(orient >> 6) & 3]; 503 | 504 | /* Chip-to-body, axes only. */ 505 | if (mpu_write_mem(FCFG_1, 3, gyro_regs)) 506 | return -1; 507 | if (mpu_write_mem(FCFG_2, 3, accel_regs)) 508 | return -1; 509 | 510 | memcpy(gyro_regs, gyro_sign, 3); 511 | memcpy(accel_regs, accel_sign, 3); 512 | if (orient & 4) { 513 | gyro_regs[0] |= 1; 514 | accel_regs[0] |= 1; 515 | } 516 | if (orient & 0x20) { 517 | gyro_regs[1] |= 1; 518 | accel_regs[1] |= 1; 519 | } 520 | if (orient & 0x100) { 521 | gyro_regs[2] |= 1; 522 | accel_regs[2] |= 1; 523 | } 524 | 525 | /* Chip-to-body, sign only. */ 526 | if (mpu_write_mem(FCFG_3, 3, gyro_regs)) 527 | return -1; 528 | if (mpu_write_mem(FCFG_7, 3, accel_regs)) 529 | return -1; 530 | dmp.orient = orient; 531 | return 0; 532 | } 533 | 534 | /** 535 | * @brief Push gyro biases to the DMP. 536 | * Because the gyro integration is handled in the DMP, any gyro biases 537 | * calculated by the MPL should be pushed down to DMP memory to remove 538 | * 3-axis quaternion drift. 539 | * \n NOTE: If the DMP-based gyro calibration is enabled, the DMP will 540 | * overwrite the biases written to this location once a new one is computed. 541 | * @param[in] bias Gyro biases in q16. 542 | * @return 0 if successful. 543 | */ 544 | int dmp_set_gyro_bias(long *bias) 545 | { 546 | long gyro_bias_body[3]; 547 | unsigned char regs[4]; 548 | 549 | gyro_bias_body[0] = bias[dmp.orient & 3]; 550 | if (dmp.orient & 4) 551 | gyro_bias_body[0] *= -1; 552 | gyro_bias_body[1] = bias[(dmp.orient >> 3) & 3]; 553 | if (dmp.orient & 0x20) 554 | gyro_bias_body[1] *= -1; 555 | gyro_bias_body[2] = bias[(dmp.orient >> 6) & 3]; 556 | if (dmp.orient & 0x100) 557 | gyro_bias_body[2] *= -1; 558 | 559 | #ifdef EMPL_NO_64BIT 560 | gyro_bias_body[0] = (long)(((float)gyro_bias_body[0] * GYRO_SF) / 1073741824.f); 561 | gyro_bias_body[1] = (long)(((float)gyro_bias_body[1] * GYRO_SF) / 1073741824.f); 562 | gyro_bias_body[2] = (long)(((float)gyro_bias_body[2] * GYRO_SF) / 1073741824.f); 563 | #else 564 | gyro_bias_body[0] = (long)(((long long)gyro_bias_body[0] * GYRO_SF) >> 30); 565 | gyro_bias_body[1] = (long)(((long long)gyro_bias_body[1] * GYRO_SF) >> 30); 566 | gyro_bias_body[2] = (long)(((long long)gyro_bias_body[2] * GYRO_SF) >> 30); 567 | #endif 568 | 569 | regs[0] = (unsigned char)((gyro_bias_body[0] >> 24) & 0xFF); 570 | regs[1] = (unsigned char)((gyro_bias_body[0] >> 16) & 0xFF); 571 | regs[2] = (unsigned char)((gyro_bias_body[0] >> 8) & 0xFF); 572 | regs[3] = (unsigned char)(gyro_bias_body[0] & 0xFF); 573 | if (mpu_write_mem(D_EXT_GYRO_BIAS_X, 4, regs)) 574 | return -1; 575 | 576 | regs[0] = (unsigned char)((gyro_bias_body[1] >> 24) & 0xFF); 577 | regs[1] = (unsigned char)((gyro_bias_body[1] >> 16) & 0xFF); 578 | regs[2] = (unsigned char)((gyro_bias_body[1] >> 8) & 0xFF); 579 | regs[3] = (unsigned char)(gyro_bias_body[1] & 0xFF); 580 | if (mpu_write_mem(D_EXT_GYRO_BIAS_Y, 4, regs)) 581 | return -1; 582 | 583 | regs[0] = (unsigned char)((gyro_bias_body[2] >> 24) & 0xFF); 584 | regs[1] = (unsigned char)((gyro_bias_body[2] >> 16) & 0xFF); 585 | regs[2] = (unsigned char)((gyro_bias_body[2] >> 8) & 0xFF); 586 | regs[3] = (unsigned char)(gyro_bias_body[2] & 0xFF); 587 | return mpu_write_mem(D_EXT_GYRO_BIAS_Z, 4, regs); 588 | } 589 | 590 | /** 591 | * @brief Push accel biases to the DMP. 592 | * These biases will be removed from the DMP 6-axis quaternion. 593 | * @param[in] bias Accel biases in q16. 594 | * @return 0 if successful. 595 | */ 596 | int dmp_set_accel_bias(long *bias) 597 | { 598 | long accel_bias_body[3]; 599 | unsigned char regs[12]; 600 | long long accel_sf; 601 | unsigned short accel_sens; 602 | 603 | mpu_get_accel_sens(&accel_sens); 604 | accel_sf = (long long)accel_sens << 15; 605 | __no_operation(); 606 | 607 | accel_bias_body[0] = bias[dmp.orient & 3]; 608 | if (dmp.orient & 4) 609 | accel_bias_body[0] *= -1; 610 | accel_bias_body[1] = bias[(dmp.orient >> 3) & 3]; 611 | if (dmp.orient & 0x20) 612 | accel_bias_body[1] *= -1; 613 | accel_bias_body[2] = bias[(dmp.orient >> 6) & 3]; 614 | if (dmp.orient & 0x100) 615 | accel_bias_body[2] *= -1; 616 | 617 | #ifdef EMPL_NO_64BIT 618 | accel_bias_body[0] = (long)(((float)accel_bias_body[0] * accel_sf) / 1073741824.f); 619 | accel_bias_body[1] = (long)(((float)accel_bias_body[1] * accel_sf) / 1073741824.f); 620 | accel_bias_body[2] = (long)(((float)accel_bias_body[2] * accel_sf) / 1073741824.f); 621 | #else 622 | accel_bias_body[0] = (long)(((long long)accel_bias_body[0] * accel_sf) >> 30); 623 | accel_bias_body[1] = (long)(((long long)accel_bias_body[1] * accel_sf) >> 30); 624 | accel_bias_body[2] = (long)(((long long)accel_bias_body[2] * accel_sf) >> 30); 625 | #endif 626 | 627 | regs[0] = (unsigned char)((accel_bias_body[0] >> 24) & 0xFF); 628 | regs[1] = (unsigned char)((accel_bias_body[0] >> 16) & 0xFF); 629 | regs[2] = (unsigned char)((accel_bias_body[0] >> 8) & 0xFF); 630 | regs[3] = (unsigned char)(accel_bias_body[0] & 0xFF); 631 | regs[4] = (unsigned char)((accel_bias_body[1] >> 24) & 0xFF); 632 | regs[5] = (unsigned char)((accel_bias_body[1] >> 16) & 0xFF); 633 | regs[6] = (unsigned char)((accel_bias_body[1] >> 8) & 0xFF); 634 | regs[7] = (unsigned char)(accel_bias_body[1] & 0xFF); 635 | regs[8] = (unsigned char)((accel_bias_body[2] >> 24) & 0xFF); 636 | regs[9] = (unsigned char)((accel_bias_body[2] >> 16) & 0xFF); 637 | regs[10] = (unsigned char)((accel_bias_body[2] >> 8) & 0xFF); 638 | regs[11] = (unsigned char)(accel_bias_body[2] & 0xFF); 639 | return mpu_write_mem(D_ACCEL_BIAS, 12, regs); 640 | } 641 | 642 | /** 643 | * @brief Set DMP output rate. 644 | * Only used when DMP is on. 645 | * @param[in] rate Desired fifo rate (Hz). 646 | * @return 0 if successful. 647 | */ 648 | int dmp_set_fifo_rate(unsigned short rate) 649 | { 650 | const unsigned char regs_end[12] = {DINAFE, DINAF2, DINAAB, 651 | 0xc4, DINAAA, DINAF1, DINADF, DINADF, 0xBB, 0xAF, DINADF, DINADF}; 652 | unsigned short div; 653 | unsigned char tmp[8]; 654 | 655 | if (rate > DMP_SAMPLE_RATE) 656 | return -1; 657 | div = DMP_SAMPLE_RATE / rate - 1; 658 | tmp[0] = (unsigned char)((div >> 8) & 0xFF); 659 | tmp[1] = (unsigned char)(div & 0xFF); 660 | if (mpu_write_mem(D_0_22, 2, tmp)) 661 | return -1; 662 | if (mpu_write_mem(CFG_6, 12, (unsigned char*)regs_end)) 663 | return -1; 664 | 665 | dmp.fifo_rate = rate; 666 | return 0; 667 | } 668 | 669 | /** 670 | * @brief Get DMP output rate. 671 | * @param[out] rate Current fifo rate (Hz). 672 | * @return 0 if successful. 673 | */ 674 | int dmp_get_fifo_rate(unsigned short *rate) 675 | { 676 | rate[0] = dmp.fifo_rate; 677 | return 0; 678 | } 679 | 680 | /** 681 | * @brief Set tap threshold for a specific axis. 682 | * @param[in] axis 1, 2, and 4 for XYZ accel, respectively. 683 | * @param[in] thresh Tap threshold, in mg/ms. 684 | * @return 0 if successful. 685 | */ 686 | int dmp_set_tap_thresh(unsigned char axis, unsigned short thresh) 687 | { 688 | unsigned char tmp[4], accel_fsr; 689 | float scaled_thresh; 690 | unsigned short dmp_thresh, dmp_thresh_2; 691 | if (!(axis & TAP_XYZ) || thresh > 1600) 692 | return -1; 693 | 694 | scaled_thresh = (float)thresh / DMP_SAMPLE_RATE; 695 | 696 | mpu_get_accel_fsr(&accel_fsr); 697 | switch (accel_fsr) { 698 | case 2: 699 | dmp_thresh = (unsigned short)(scaled_thresh * 16384); 700 | /* dmp_thresh * 0.75 */ 701 | dmp_thresh_2 = (unsigned short)(scaled_thresh * 12288); 702 | break; 703 | case 4: 704 | dmp_thresh = (unsigned short)(scaled_thresh * 8192); 705 | /* dmp_thresh * 0.75 */ 706 | dmp_thresh_2 = (unsigned short)(scaled_thresh * 6144); 707 | break; 708 | case 8: 709 | dmp_thresh = (unsigned short)(scaled_thresh * 4096); 710 | /* dmp_thresh * 0.75 */ 711 | dmp_thresh_2 = (unsigned short)(scaled_thresh * 3072); 712 | break; 713 | case 16: 714 | dmp_thresh = (unsigned short)(scaled_thresh * 2048); 715 | /* dmp_thresh * 0.75 */ 716 | dmp_thresh_2 = (unsigned short)(scaled_thresh * 1536); 717 | break; 718 | default: 719 | return -1; 720 | } 721 | tmp[0] = (unsigned char)(dmp_thresh >> 8); 722 | tmp[1] = (unsigned char)(dmp_thresh & 0xFF); 723 | tmp[2] = (unsigned char)(dmp_thresh_2 >> 8); 724 | tmp[3] = (unsigned char)(dmp_thresh_2 & 0xFF); 725 | 726 | if (axis & TAP_X) { 727 | if (mpu_write_mem(DMP_TAP_THX, 2, tmp)) 728 | return -1; 729 | if (mpu_write_mem(D_1_36, 2, tmp+2)) 730 | return -1; 731 | } 732 | if (axis & TAP_Y) { 733 | if (mpu_write_mem(DMP_TAP_THY, 2, tmp)) 734 | return -1; 735 | if (mpu_write_mem(D_1_40, 2, tmp+2)) 736 | return -1; 737 | } 738 | if (axis & TAP_Z) { 739 | if (mpu_write_mem(DMP_TAP_THZ, 2, tmp)) 740 | return -1; 741 | if (mpu_write_mem(D_1_44, 2, tmp+2)) 742 | return -1; 743 | } 744 | return 0; 745 | } 746 | 747 | /** 748 | * @brief Set which axes will register a tap. 749 | * @param[in] axis 1, 2, and 4 for XYZ, respectively. 750 | * @return 0 if successful. 751 | */ 752 | int dmp_set_tap_axes(unsigned char axis) 753 | { 754 | unsigned char tmp = 0; 755 | 756 | if (axis & TAP_X) 757 | tmp |= 0x30; 758 | if (axis & TAP_Y) 759 | tmp |= 0x0C; 760 | if (axis & TAP_Z) 761 | tmp |= 0x03; 762 | return mpu_write_mem(D_1_72, 1, &tmp); 763 | } 764 | 765 | /** 766 | * @brief Set minimum number of taps needed for an interrupt. 767 | * @param[in] min_taps Minimum consecutive taps (1-4). 768 | * @return 0 if successful. 769 | */ 770 | int dmp_set_tap_count(unsigned char min_taps) 771 | { 772 | unsigned char tmp; 773 | 774 | if (min_taps < 1) 775 | min_taps = 1; 776 | else if (min_taps > 4) 777 | min_taps = 4; 778 | 779 | tmp = min_taps - 1; 780 | return mpu_write_mem(D_1_79, 1, &tmp); 781 | } 782 | 783 | /** 784 | * @brief Set length between valid taps. 785 | * @param[in] time Milliseconds between taps. 786 | * @return 0 if successful. 787 | */ 788 | int dmp_set_tap_time(unsigned short time) 789 | { 790 | unsigned short dmp_time; 791 | unsigned char tmp[2]; 792 | 793 | dmp_time = time / (1000 / DMP_SAMPLE_RATE); 794 | tmp[0] = (unsigned char)(dmp_time >> 8); 795 | tmp[1] = (unsigned char)(dmp_time & 0xFF); 796 | return mpu_write_mem(DMP_TAPW_MIN, 2, tmp); 797 | } 798 | 799 | /** 800 | * @brief Set max time between taps to register as a multi-tap. 801 | * @param[in] time Max milliseconds between taps. 802 | * @return 0 if successful. 803 | */ 804 | int dmp_set_tap_time_multi(unsigned short time) 805 | { 806 | unsigned short dmp_time; 807 | unsigned char tmp[2]; 808 | 809 | dmp_time = time / (1000 / DMP_SAMPLE_RATE); 810 | tmp[0] = (unsigned char)(dmp_time >> 8); 811 | tmp[1] = (unsigned char)(dmp_time & 0xFF); 812 | return mpu_write_mem(D_1_218, 2, tmp); 813 | } 814 | 815 | /** 816 | * @brief Set shake rejection threshold. 817 | * If the DMP detects a gyro sample larger than @e thresh, taps are rejected. 818 | * @param[in] sf Gyro scale factor. 819 | * @param[in] thresh Gyro threshold in dps. 820 | * @return 0 if successful. 821 | */ 822 | int dmp_set_shake_reject_thresh(long sf, unsigned short thresh) 823 | { 824 | unsigned char tmp[4]; 825 | long thresh_scaled = sf / 1000 * thresh; 826 | tmp[0] = (unsigned char)(((long)thresh_scaled >> 24) & 0xFF); 827 | tmp[1] = (unsigned char)(((long)thresh_scaled >> 16) & 0xFF); 828 | tmp[2] = (unsigned char)(((long)thresh_scaled >> 8) & 0xFF); 829 | tmp[3] = (unsigned char)((long)thresh_scaled & 0xFF); 830 | return mpu_write_mem(D_1_92, 4, tmp); 831 | } 832 | 833 | /** 834 | * @brief Set shake rejection time. 835 | * Sets the length of time that the gyro must be outside of the threshold set 836 | * by @e gyro_set_shake_reject_thresh before taps are rejected. A mandatory 837 | * 60 ms is added to this parameter. 838 | * @param[in] time Time in milliseconds. 839 | * @return 0 if successful. 840 | */ 841 | int dmp_set_shake_reject_time(unsigned short time) 842 | { 843 | unsigned char tmp[2]; 844 | 845 | time /= (1000 / DMP_SAMPLE_RATE); 846 | tmp[0] = time >> 8; 847 | tmp[1] = time & 0xFF; 848 | return mpu_write_mem(D_1_90,2,tmp); 849 | } 850 | 851 | /** 852 | * @brief Set shake rejection timeout. 853 | * Sets the length of time after a shake rejection that the gyro must stay 854 | * inside of the threshold before taps can be detected again. A mandatory 855 | * 60 ms is added to this parameter. 856 | * @param[in] time Time in milliseconds. 857 | * @return 0 if successful. 858 | */ 859 | int dmp_set_shake_reject_timeout(unsigned short time) 860 | { 861 | unsigned char tmp[2]; 862 | 863 | time /= (1000 / DMP_SAMPLE_RATE); 864 | tmp[0] = time >> 8; 865 | tmp[1] = time & 0xFF; 866 | return mpu_write_mem(D_1_88,2,tmp); 867 | } 868 | 869 | /** 870 | * @brief Get current step count. 871 | * @param[out] count Number of steps detected. 872 | * @return 0 if successful. 873 | */ 874 | int dmp_get_pedometer_step_count(unsigned long *count) 875 | { 876 | unsigned char tmp[4]; 877 | if (!count) 878 | return -1; 879 | 880 | if (mpu_read_mem(D_PEDSTD_STEPCTR, 4, tmp)) 881 | return -1; 882 | 883 | count[0] = ((unsigned long)tmp[0] << 24) | ((unsigned long)tmp[1] << 16) | 884 | ((unsigned long)tmp[2] << 8) | tmp[3]; 885 | return 0; 886 | } 887 | 888 | /** 889 | * @brief Overwrite current step count. 890 | * WARNING: This function writes to DMP memory and could potentially encounter 891 | * a race condition if called while the pedometer is enabled. 892 | * @param[in] count New step count. 893 | * @return 0 if successful. 894 | */ 895 | int dmp_set_pedometer_step_count(unsigned long count) 896 | { 897 | unsigned char tmp[4]; 898 | 899 | tmp[0] = (unsigned char)((count >> 24) & 0xFF); 900 | tmp[1] = (unsigned char)((count >> 16) & 0xFF); 901 | tmp[2] = (unsigned char)((count >> 8) & 0xFF); 902 | tmp[3] = (unsigned char)(count & 0xFF); 903 | return mpu_write_mem(D_PEDSTD_STEPCTR, 4, tmp); 904 | } 905 | 906 | /** 907 | * @brief Get duration of walking time. 908 | * @param[in] time Walk time in milliseconds. 909 | * @return 0 if successful. 910 | */ 911 | int dmp_get_pedometer_walk_time(unsigned long *time) 912 | { 913 | unsigned char tmp[4]; 914 | if (!time) 915 | return -1; 916 | 917 | if (mpu_read_mem(D_PEDSTD_TIMECTR, 4, tmp)) 918 | return -1; 919 | 920 | time[0] = (((unsigned long)tmp[0] << 24) | ((unsigned long)tmp[1] << 16) | 921 | ((unsigned long)tmp[2] << 8) | tmp[3]) * 20; 922 | return 0; 923 | } 924 | 925 | /** 926 | * @brief Overwrite current walk time. 927 | * WARNING: This function writes to DMP memory and could potentially encounter 928 | * a race condition if called while the pedometer is enabled. 929 | * @param[in] time New walk time in milliseconds. 930 | */ 931 | int dmp_set_pedometer_walk_time(unsigned long time) 932 | { 933 | unsigned char tmp[4]; 934 | 935 | time /= 20; 936 | 937 | tmp[0] = (unsigned char)((time >> 24) & 0xFF); 938 | tmp[1] = (unsigned char)((time >> 16) & 0xFF); 939 | tmp[2] = (unsigned char)((time >> 8) & 0xFF); 940 | tmp[3] = (unsigned char)(time & 0xFF); 941 | return mpu_write_mem(D_PEDSTD_TIMECTR, 4, tmp); 942 | } 943 | 944 | /** 945 | * @brief Enable DMP features. 946 | * The following \#define's are used in the input mask: 947 | * \n DMP_FEATURE_TAP 948 | * \n DMP_FEATURE_ANDROID_ORIENT 949 | * \n DMP_FEATURE_LP_QUAT 950 | * \n DMP_FEATURE_6X_LP_QUAT 951 | * \n DMP_FEATURE_GYRO_CAL 952 | * \n DMP_FEATURE_SEND_RAW_ACCEL 953 | * \n DMP_FEATURE_SEND_RAW_GYRO 954 | * \n NOTE: DMP_FEATURE_LP_QUAT and DMP_FEATURE_6X_LP_QUAT are mutually 955 | * exclusive. 956 | * \n NOTE: DMP_FEATURE_SEND_RAW_GYRO and DMP_FEATURE_SEND_CAL_GYRO are also 957 | * mutually exclusive. 958 | * @param[in] mask Mask of features to enable. 959 | * @return 0 if successful. 960 | */ 961 | int dmp_enable_feature(unsigned short mask) 962 | { 963 | unsigned char tmp[10]; 964 | 965 | /* TODO: All of these settings can probably be integrated into the default 966 | * DMP image. 967 | */ 968 | /* Set integration scale factor. */ 969 | tmp[0] = (unsigned char)((GYRO_SF >> 24) & 0xFF); 970 | tmp[1] = (unsigned char)((GYRO_SF >> 16) & 0xFF); 971 | tmp[2] = (unsigned char)((GYRO_SF >> 8) & 0xFF); 972 | tmp[3] = (unsigned char)(GYRO_SF & 0xFF); 973 | mpu_write_mem(D_0_104, 4, tmp); 974 | 975 | /* Send sensor data to the FIFO. */ 976 | tmp[0] = 0xA3; 977 | if (mask & DMP_FEATURE_SEND_RAW_ACCEL) { 978 | tmp[1] = 0xC0; 979 | tmp[2] = 0xC8; 980 | tmp[3] = 0xC2; 981 | } else { 982 | tmp[1] = 0xA3; 983 | tmp[2] = 0xA3; 984 | tmp[3] = 0xA3; 985 | } 986 | if (mask & DMP_FEATURE_SEND_ANY_GYRO) { 987 | tmp[4] = 0xC4; 988 | tmp[5] = 0xCC; 989 | tmp[6] = 0xC6; 990 | } else { 991 | tmp[4] = 0xA3; 992 | tmp[5] = 0xA3; 993 | tmp[6] = 0xA3; 994 | } 995 | tmp[7] = 0xA3; 996 | tmp[8] = 0xA3; 997 | tmp[9] = 0xA3; 998 | mpu_write_mem(CFG_15,10,tmp); 999 | 1000 | /* Send gesture data to the FIFO. */ 1001 | if (mask & (DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT)) 1002 | tmp[0] = DINA20; 1003 | else 1004 | tmp[0] = 0xD8; 1005 | mpu_write_mem(CFG_27,1,tmp); 1006 | 1007 | if (mask & DMP_FEATURE_GYRO_CAL) 1008 | dmp_enable_gyro_cal(1); 1009 | else 1010 | dmp_enable_gyro_cal(0); 1011 | 1012 | if (mask & DMP_FEATURE_SEND_ANY_GYRO) { 1013 | if (mask & DMP_FEATURE_SEND_CAL_GYRO) { 1014 | tmp[0] = 0xB2; 1015 | tmp[1] = 0x8B; 1016 | tmp[2] = 0xB6; 1017 | tmp[3] = 0x9B; 1018 | } else { 1019 | tmp[0] = DINAC0; 1020 | tmp[1] = DINA80; 1021 | tmp[2] = DINAC2; 1022 | tmp[3] = DINA90; 1023 | } 1024 | mpu_write_mem(CFG_GYRO_RAW_DATA, 4, tmp); 1025 | } 1026 | 1027 | if (mask & DMP_FEATURE_TAP) { 1028 | /* Enable tap. */ 1029 | tmp[0] = 0xF8; 1030 | mpu_write_mem(CFG_20, 1, tmp); 1031 | dmp_set_tap_thresh(TAP_XYZ, 250); 1032 | dmp_set_tap_axes(TAP_XYZ); 1033 | dmp_set_tap_count(1); 1034 | dmp_set_tap_time(100); 1035 | dmp_set_tap_time_multi(500); 1036 | 1037 | dmp_set_shake_reject_thresh(GYRO_SF, 200); 1038 | dmp_set_shake_reject_time(40); 1039 | dmp_set_shake_reject_timeout(10); 1040 | } else { 1041 | tmp[0] = 0xD8; 1042 | mpu_write_mem(CFG_20, 1, tmp); 1043 | } 1044 | 1045 | if (mask & DMP_FEATURE_ANDROID_ORIENT) { 1046 | tmp[0] = 0xD9; 1047 | } else 1048 | tmp[0] = 0xD8; 1049 | mpu_write_mem(CFG_ANDROID_ORIENT_INT, 1, tmp); 1050 | 1051 | if (mask & DMP_FEATURE_LP_QUAT) 1052 | dmp_enable_lp_quat(1); 1053 | else 1054 | dmp_enable_lp_quat(0); 1055 | 1056 | if (mask & DMP_FEATURE_6X_LP_QUAT) 1057 | dmp_enable_6x_lp_quat(1); 1058 | else 1059 | dmp_enable_6x_lp_quat(0); 1060 | 1061 | /* Pedometer is always enabled. */ 1062 | dmp.feature_mask = mask | DMP_FEATURE_PEDOMETER; 1063 | mpu_reset_fifo(); 1064 | 1065 | dmp.packet_length = 0; 1066 | if (mask & DMP_FEATURE_SEND_RAW_ACCEL) 1067 | dmp.packet_length += 6; 1068 | if (mask & DMP_FEATURE_SEND_ANY_GYRO) 1069 | dmp.packet_length += 6; 1070 | if (mask & (DMP_FEATURE_LP_QUAT | DMP_FEATURE_6X_LP_QUAT)) 1071 | dmp.packet_length += 16; 1072 | if (mask & (DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT)) 1073 | dmp.packet_length += 4; 1074 | 1075 | return 0; 1076 | } 1077 | 1078 | /** 1079 | * @brief Get list of currently enabled DMP features. 1080 | * @param[out] Mask of enabled features. 1081 | * @return 0 if successful. 1082 | */ 1083 | int dmp_get_enabled_features(unsigned short *mask) 1084 | { 1085 | mask[0] = dmp.feature_mask; 1086 | return 0; 1087 | } 1088 | 1089 | /** 1090 | * @brief Calibrate the gyro data in the DMP. 1091 | * After eight seconds of no motion, the DMP will compute gyro biases and 1092 | * subtract them from the quaternion output. If @e dmp_enable_feature is 1093 | * called with @e DMP_FEATURE_SEND_CAL_GYRO, the biases will also be 1094 | * subtracted from the gyro output. 1095 | * @param[in] enable 1 to enable gyro calibration. 1096 | * @return 0 if successful. 1097 | */ 1098 | int dmp_enable_gyro_cal(unsigned char enable) 1099 | { 1100 | if (enable) { 1101 | unsigned char regs[9] = {0xb8, 0xaa, 0xb3, 0x8d, 0xb4, 0x98, 0x0d, 0x35, 0x5d}; 1102 | return mpu_write_mem(CFG_MOTION_BIAS, 9, regs); 1103 | } else { 1104 | unsigned char regs[9] = {0xb8, 0xaa, 0xaa, 0xaa, 0xb0, 0x88, 0xc3, 0xc5, 0xc7}; 1105 | return mpu_write_mem(CFG_MOTION_BIAS, 9, regs); 1106 | } 1107 | } 1108 | 1109 | /** 1110 | * @brief Generate 3-axis quaternions from the DMP. 1111 | * In this driver, the 3-axis and 6-axis DMP quaternion features are mutually 1112 | * exclusive. 1113 | * @param[in] enable 1 to enable 3-axis quaternion. 1114 | * @return 0 if successful. 1115 | */ 1116 | int dmp_enable_lp_quat(unsigned char enable) 1117 | { 1118 | unsigned char regs[4]; 1119 | if (enable) { 1120 | regs[0] = DINBC0; 1121 | regs[1] = DINBC2; 1122 | regs[2] = DINBC4; 1123 | regs[3] = DINBC6; 1124 | } 1125 | else 1126 | memset(regs, 0x8B, 4); 1127 | 1128 | mpu_write_mem(CFG_LP_QUAT, 4, regs); 1129 | 1130 | return mpu_reset_fifo(); 1131 | } 1132 | 1133 | /** 1134 | * @brief Generate 6-axis quaternions from the DMP. 1135 | * In this driver, the 3-axis and 6-axis DMP quaternion features are mutually 1136 | * exclusive. 1137 | * @param[in] enable 1 to enable 6-axis quaternion. 1138 | * @return 0 if successful. 1139 | */ 1140 | int dmp_enable_6x_lp_quat(unsigned char enable) 1141 | { 1142 | unsigned char regs[4]; 1143 | if (enable) { 1144 | regs[0] = DINA20; 1145 | regs[1] = DINA28; 1146 | regs[2] = DINA30; 1147 | regs[3] = DINA38; 1148 | } else 1149 | memset(regs, 0xA3, 4); 1150 | 1151 | mpu_write_mem(CFG_8, 4, regs); 1152 | 1153 | return mpu_reset_fifo(); 1154 | } 1155 | 1156 | /** 1157 | * @brief Decode the four-byte gesture data and execute any callbacks. 1158 | * @param[in] gesture Gesture data from DMP packet. 1159 | * @return 0 if successful. 1160 | */ 1161 | static int decode_gesture(unsigned char *gesture) 1162 | { 1163 | unsigned char tap, android_orient; 1164 | 1165 | android_orient = gesture[3] & 0xC0; 1166 | tap = 0x3F & gesture[3]; 1167 | 1168 | if (gesture[1] & INT_SRC_TAP) { 1169 | unsigned char direction, count; 1170 | direction = tap >> 3; 1171 | count = (tap % 8) + 1; 1172 | if (dmp.tap_cb) 1173 | dmp.tap_cb(direction, count); 1174 | } 1175 | 1176 | if (gesture[1] & INT_SRC_ANDROID_ORIENT) { 1177 | if (dmp.android_orient_cb) 1178 | dmp.android_orient_cb(android_orient >> 6); 1179 | } 1180 | 1181 | return 0; 1182 | } 1183 | 1184 | /** 1185 | * @brief Specify when a DMP interrupt should occur. 1186 | * A DMP interrupt can be configured to trigger on either of the two 1187 | * conditions below: 1188 | * \n a. One FIFO period has elapsed (set by @e mpu_set_sample_rate). 1189 | * \n b. A tap event has been detected. 1190 | * @param[in] mode DMP_INT_GESTURE or DMP_INT_CONTINUOUS. 1191 | * @return 0 if successful. 1192 | */ 1193 | int dmp_set_interrupt_mode(unsigned char mode) 1194 | { 1195 | const unsigned char regs_continuous[11] = 1196 | {0xd8, 0xb1, 0xb9, 0xf3, 0x8b, 0xa3, 0x91, 0xb6, 0x09, 0xb4, 0xd9}; 1197 | const unsigned char regs_gesture[11] = 1198 | {0xda, 0xb1, 0xb9, 0xf3, 0x8b, 0xa3, 0x91, 0xb6, 0xda, 0xb4, 0xda}; 1199 | 1200 | switch (mode) { 1201 | case DMP_INT_CONTINUOUS: 1202 | return mpu_write_mem(CFG_FIFO_ON_EVENT, 11, 1203 | (unsigned char*)regs_continuous); 1204 | case DMP_INT_GESTURE: 1205 | return mpu_write_mem(CFG_FIFO_ON_EVENT, 11, 1206 | (unsigned char*)regs_gesture); 1207 | default: 1208 | return -1; 1209 | } 1210 | } 1211 | 1212 | /** 1213 | * @brief Get one packet from the FIFO. 1214 | * If @e sensors does not contain a particular sensor, disregard the data 1215 | * returned to that pointer. 1216 | * \n @e sensors can contain a combination of the following flags: 1217 | * \n INV_X_GYRO, INV_Y_GYRO, INV_Z_GYRO 1218 | * \n INV_XYZ_GYRO 1219 | * \n INV_XYZ_ACCEL 1220 | * \n INV_WXYZ_QUAT 1221 | * \n If the FIFO has no new data, @e sensors will be zero. 1222 | * \n If the FIFO is disabled, @e sensors will be zero and this function will 1223 | * return a non-zero error code. 1224 | * @param[out] gyro Gyro data in hardware units. 1225 | * @param[out] accel Accel data in hardware units. 1226 | * @param[out] quat 3-axis quaternion data in hardware units. 1227 | * @param[out] timestamp Timestamp in milliseconds. 1228 | * @param[out] sensors Mask of sensors read from FIFO. 1229 | * @param[out] more Number of remaining packets. 1230 | * @return 0 if successful. 1231 | */ 1232 | int dmp_read_fifo(short *gyro, short *accel, long *quat, 1233 | unsigned long *timestamp, short *sensors, unsigned char *more) 1234 | { 1235 | unsigned char fifo_data[MAX_PACKET_LENGTH]; 1236 | unsigned char ii = 0; 1237 | 1238 | /* TODO: sensors[0] only changes when dmp_enable_feature is called. We can 1239 | * cache this value and save some cycles. 1240 | */ 1241 | sensors[0] = 0; 1242 | 1243 | /* Get a packet. */ 1244 | if (mpu_read_fifo_stream(dmp.packet_length, fifo_data, more)) 1245 | return -1; 1246 | 1247 | /* Parse DMP packet. */ 1248 | if (dmp.feature_mask & (DMP_FEATURE_LP_QUAT | DMP_FEATURE_6X_LP_QUAT)) { 1249 | #ifdef FIFO_CORRUPTION_CHECK 1250 | long quat_q14[4], quat_mag_sq; 1251 | #endif 1252 | quat[0] = ((long)fifo_data[0] << 24) | ((long)fifo_data[1] << 16) | 1253 | ((long)fifo_data[2] << 8) | fifo_data[3]; 1254 | quat[1] = ((long)fifo_data[4] << 24) | ((long)fifo_data[5] << 16) | 1255 | ((long)fifo_data[6] << 8) | fifo_data[7]; 1256 | quat[2] = ((long)fifo_data[8] << 24) | ((long)fifo_data[9] << 16) | 1257 | ((long)fifo_data[10] << 8) | fifo_data[11]; 1258 | quat[3] = ((long)fifo_data[12] << 24) | ((long)fifo_data[13] << 16) | 1259 | ((long)fifo_data[14] << 8) | fifo_data[15]; 1260 | ii += 16; 1261 | #ifdef FIFO_CORRUPTION_CHECK 1262 | /* We can detect a corrupted FIFO by monitoring the quaternion data and 1263 | * ensuring that the magnitude is always normalized to one. This 1264 | * shouldn't happen in normal operation, but if an I2C error occurs, 1265 | * the FIFO reads might become misaligned. 1266 | * 1267 | * Let's start by scaling down the quaternion data to avoid long long 1268 | * math. 1269 | */ 1270 | quat_q14[0] = quat[0] >> 16; 1271 | quat_q14[1] = quat[1] >> 16; 1272 | quat_q14[2] = quat[2] >> 16; 1273 | quat_q14[3] = quat[3] >> 16; 1274 | quat_mag_sq = quat_q14[0] * quat_q14[0] + quat_q14[1] * quat_q14[1] + 1275 | quat_q14[2] * quat_q14[2] + quat_q14[3] * quat_q14[3]; 1276 | if ((quat_mag_sq < QUAT_MAG_SQ_MIN) || 1277 | (quat_mag_sq > QUAT_MAG_SQ_MAX)) { 1278 | /* Quaternion is outside of the acceptable threshold. */ 1279 | mpu_reset_fifo(); 1280 | sensors[0] = 0; 1281 | return -1; 1282 | } 1283 | sensors[0] |= INV_WXYZ_QUAT; 1284 | #endif 1285 | } 1286 | 1287 | if (dmp.feature_mask & DMP_FEATURE_SEND_RAW_ACCEL) { 1288 | accel[0] = ((short)fifo_data[ii+0] << 8) | fifo_data[ii+1]; 1289 | accel[1] = ((short)fifo_data[ii+2] << 8) | fifo_data[ii+3]; 1290 | accel[2] = ((short)fifo_data[ii+4] << 8) | fifo_data[ii+5]; 1291 | ii += 6; 1292 | sensors[0] |= INV_XYZ_ACCEL; 1293 | } 1294 | 1295 | if (dmp.feature_mask & DMP_FEATURE_SEND_ANY_GYRO) { 1296 | gyro[0] = ((short)fifo_data[ii+0] << 8) | fifo_data[ii+1]; 1297 | gyro[1] = ((short)fifo_data[ii+2] << 8) | fifo_data[ii+3]; 1298 | gyro[2] = ((short)fifo_data[ii+4] << 8) | fifo_data[ii+5]; 1299 | ii += 6; 1300 | sensors[0] |= INV_XYZ_GYRO; 1301 | } 1302 | 1303 | /* Gesture data is at the end of the DMP packet. Parse it and call 1304 | * the gesture callbacks (if registered). 1305 | */ 1306 | if (dmp.feature_mask & (DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT)) 1307 | decode_gesture(fifo_data + ii); 1308 | 1309 | get_ms(timestamp); 1310 | return 0; 1311 | } 1312 | 1313 | /** 1314 | * @brief Register a function to be executed on a tap event. 1315 | * The tap direction is represented by one of the following: 1316 | * \n TAP_X_UP 1317 | * \n TAP_X_DOWN 1318 | * \n TAP_Y_UP 1319 | * \n TAP_Y_DOWN 1320 | * \n TAP_Z_UP 1321 | * \n TAP_Z_DOWN 1322 | * @param[in] func Callback function. 1323 | * @return 0 if successful. 1324 | */ 1325 | int dmp_register_tap_cb(void (*func)(unsigned char, unsigned char)) 1326 | { 1327 | dmp.tap_cb = func; 1328 | return 0; 1329 | } 1330 | 1331 | /** 1332 | * @brief Register a function to be executed on a android orientation event. 1333 | * @param[in] func Callback function. 1334 | * @return 0 if successful. 1335 | */ 1336 | int dmp_register_android_orient_cb(void (*func)(unsigned char)) 1337 | { 1338 | dmp.android_orient_cb = func; 1339 | return 0; 1340 | } 1341 | 1342 | /** 1343 | * @} 1344 | */ 1345 | 1346 | -------------------------------------------------------------------------------- /src/util/inv_mpu_dmp_motion_driver.h: -------------------------------------------------------------------------------- 1 | /* 2 | $License: 3 | Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. 4 | See included License.txt for License information. 5 | $ 6 | */ 7 | /** 8 | * @addtogroup DRIVERS Sensor Driver Layer 9 | * @brief Hardware drivers to communicate with sensors via I2C. 10 | * 11 | * @{ 12 | * @file inv_mpu_dmp_motion_driver.h 13 | * @brief DMP image and interface functions. 14 | * @details All functions are preceded by the dmp_ prefix to 15 | * differentiate among MPL and general driver function calls. 16 | */ 17 | #ifndef _INV_MPU_DMP_MOTION_DRIVER_H_ 18 | #define _INV_MPU_DMP_MOTION_DRIVER_H_ 19 | 20 | #define TAP_X (0x01) 21 | #define TAP_Y (0x02) 22 | #define TAP_Z (0x04) 23 | #define TAP_XYZ (0x07) 24 | 25 | #define TAP_X_UP (0x01) 26 | #define TAP_X_DOWN (0x02) 27 | #define TAP_Y_UP (0x03) 28 | #define TAP_Y_DOWN (0x04) 29 | #define TAP_Z_UP (0x05) 30 | #define TAP_Z_DOWN (0x06) 31 | 32 | #define ANDROID_ORIENT_PORTRAIT (0x00) 33 | #define ANDROID_ORIENT_LANDSCAPE (0x01) 34 | #define ANDROID_ORIENT_REVERSE_PORTRAIT (0x02) 35 | #define ANDROID_ORIENT_REVERSE_LANDSCAPE (0x03) 36 | 37 | #define DMP_INT_GESTURE (0x01) 38 | #define DMP_INT_CONTINUOUS (0x02) 39 | 40 | #define DMP_FEATURE_TAP (0x001) 41 | #define DMP_FEATURE_ANDROID_ORIENT (0x002) 42 | #define DMP_FEATURE_LP_QUAT (0x004) 43 | #define DMP_FEATURE_PEDOMETER (0x008) 44 | #define DMP_FEATURE_6X_LP_QUAT (0x010) 45 | #define DMP_FEATURE_GYRO_CAL (0x020) 46 | #define DMP_FEATURE_SEND_RAW_ACCEL (0x040) 47 | #define DMP_FEATURE_SEND_RAW_GYRO (0x080) 48 | #define DMP_FEATURE_SEND_CAL_GYRO (0x100) 49 | 50 | #define INV_WXYZ_QUAT (0x100) 51 | 52 | /* Set up functions. */ 53 | int dmp_load_motion_driver_firmware(void); 54 | int dmp_set_fifo_rate(unsigned short rate); 55 | int dmp_get_fifo_rate(unsigned short *rate); 56 | int dmp_enable_feature(unsigned short mask); 57 | int dmp_get_enabled_features(unsigned short *mask); 58 | int dmp_set_interrupt_mode(unsigned char mode); 59 | int dmp_set_orientation(unsigned short orient); 60 | int dmp_set_gyro_bias(long *bias); 61 | int dmp_set_accel_bias(long *bias); 62 | 63 | /* Tap functions. */ 64 | int dmp_register_tap_cb(void (*func)(unsigned char, unsigned char)); 65 | int dmp_set_tap_thresh(unsigned char axis, unsigned short thresh); 66 | int dmp_set_tap_axes(unsigned char axis); 67 | int dmp_set_tap_count(unsigned char min_taps); 68 | int dmp_set_tap_time(unsigned short time); 69 | int dmp_set_tap_time_multi(unsigned short time); 70 | int dmp_set_shake_reject_thresh(long sf, unsigned short thresh); 71 | int dmp_set_shake_reject_time(unsigned short time); 72 | int dmp_set_shake_reject_timeout(unsigned short time); 73 | 74 | /* Android orientation functions. */ 75 | int dmp_register_android_orient_cb(void (*func)(unsigned char)); 76 | 77 | /* LP quaternion functions. */ 78 | int dmp_enable_lp_quat(unsigned char enable); 79 | int dmp_enable_6x_lp_quat(unsigned char enable); 80 | 81 | /* Pedometer functions. */ 82 | int dmp_get_pedometer_step_count(unsigned long *count); 83 | int dmp_set_pedometer_step_count(unsigned long count); 84 | int dmp_get_pedometer_walk_time(unsigned long *time); 85 | int dmp_set_pedometer_walk_time(unsigned long time); 86 | 87 | /* DMP gyro calibration functions. */ 88 | int dmp_enable_gyro_cal(unsigned char enable); 89 | 90 | /* Read function. This function should be called whenever the MPU interrupt is 91 | * detected. 92 | */ 93 | int dmp_read_fifo(short *gyro, short *accel, long *quat, 94 | unsigned long *timestamp, short *sensors, unsigned char *more); 95 | 96 | #endif /* #ifndef _INV_MPU_DMP_MOTION_DRIVER_H_ */ 97 | 98 | --------------------------------------------------------------------------------