diff --git a/README.md b/README.md index 9c0699a..ea14c7a 100644 --- a/README.md +++ b/README.md @@ -1,8 +1,7 @@ SparkFun MPU-9250 Digital Motion Processor (DMP) Arduino Library ======================================== -![SparkFun MPU-9250](https://cdn.sparkfun.com/assets/parts/1/1/3/0/6/13762-SparkFun_IMU_Breakout_-_MPU-9250-00.jpg) - +[SparkFun MPU-9250](https://cdn.sparkfun.com/assets/parts/1/1/3/0/6/13762-SparkFun_IMU_Breakout_-_MPU-9250-00.jpg) [*SparkFun MPU-9250 (SEN-13762)*](https://www.sparkfun.com/products/13762) @@ -16,7 +15,7 @@ Advanced Arduino library for the Invensense MPU-9250 inertial measurement unit ( 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. -**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. +**Note**: This library fork is tested on **SAMD, ESP8266, ESP32 and STM32L0 Arduino Cores**. It's a major part of the [SparkFun 9DoF Razor IMU M0](https://www.sparkfun.com/products/14001) firmware. 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. diff --git a/examples/MPU9250_Basic/MPU9250_Basic.ino b/examples/MPU9250_Basic/MPU9250_Basic.ino index 1d7f78b..af17dfa 100644 --- a/examples/MPU9250_Basic/MPU9250_Basic.ino +++ b/examples/MPU9250_Basic/MPU9250_Basic.ino @@ -17,7 +17,11 @@ Supported Platforms: *************************************************************/ #include -#define SerialPort SerialUSB +#ifdef defined(SAMD) + #define SerialPort SerialUSB +#else + #define SerialPort Serial +#endif MPU9250_DMP imu; diff --git a/examples/MPU9250_Basic_Interrupt/MPU9250_Basic_Interrupt.ino b/examples/MPU9250_Basic_Interrupt/MPU9250_Basic_Interrupt.ino index cf9cd5e..8a0ebe2 100644 --- a/examples/MPU9250_Basic_Interrupt/MPU9250_Basic_Interrupt.ino +++ b/examples/MPU9250_Basic_Interrupt/MPU9250_Basic_Interrupt.ino @@ -19,7 +19,12 @@ Supported Platforms: *************************************************************/ #include -#define SerialPort SerialUSB +#ifdef defined(SAMD) + #define SerialPort SerialUSB +#else + #define SerialPort Serial +#endif + #define INTERRUPT_PIN 4 MPU9250_DMP imu; diff --git a/examples/MPU9250_DMP_Gyro_Cal/MPU9250_DMP_Gyro_Cal.ino b/examples/MPU9250_DMP_Gyro_Cal/MPU9250_DMP_Gyro_Cal.ino index 85c4cff..ea78cde 100644 --- a/examples/MPU9250_DMP_Gyro_Cal/MPU9250_DMP_Gyro_Cal.ino +++ b/examples/MPU9250_DMP_Gyro_Cal/MPU9250_DMP_Gyro_Cal.ino @@ -19,7 +19,11 @@ Supported Platforms: *************************************************************/ #include -#define SerialPort SerialUSB +#ifdef defined(SAMD) + #define SerialPort SerialUSB +#else + #define SerialPort Serial +#endif MPU9250_DMP imu; diff --git a/examples/MPU9250_DMP_Orientation/MPU9250_DMP_Orientation.ino b/examples/MPU9250_DMP_Orientation/MPU9250_DMP_Orientation.ino index ad3280e..73a6206 100644 --- a/examples/MPU9250_DMP_Orientation/MPU9250_DMP_Orientation.ino +++ b/examples/MPU9250_DMP_Orientation/MPU9250_DMP_Orientation.ino @@ -17,7 +17,11 @@ Supported Platforms: *************************************************************/ #include -#define SerialPort SerialUSB +#ifdef defined(SAMD) + #define SerialPort SerialUSB +#else + #define SerialPort Serial +#endif MPU9250_DMP imu; diff --git a/examples/MPU9250_DMP_Pedometer/MPU9250_DMP_Pedometer.ino b/examples/MPU9250_DMP_Pedometer/MPU9250_DMP_Pedometer.ino index 16a0586..95a128d 100644 --- a/examples/MPU9250_DMP_Pedometer/MPU9250_DMP_Pedometer.ino +++ b/examples/MPU9250_DMP_Pedometer/MPU9250_DMP_Pedometer.ino @@ -20,7 +20,11 @@ Supported Platforms: *************************************************************/ #include -#define SerialPort SerialUSB +#ifdef defined(SAMD) + #define SerialPort SerialUSB +#else + #define SerialPort Serial +#endif MPU9250_DMP imu; diff --git a/examples/MPU9250_DMP_Quaternion/MPU9250_DMP_Quaternion.ino b/examples/MPU9250_DMP_Quaternion/MPU9250_DMP_Quaternion.ino index 410339e..dfa4ed2 100644 --- a/examples/MPU9250_DMP_Quaternion/MPU9250_DMP_Quaternion.ino +++ b/examples/MPU9250_DMP_Quaternion/MPU9250_DMP_Quaternion.ino @@ -23,7 +23,11 @@ Supported Platforms: *************************************************************/ #include -#define SerialPort SerialUSB +#ifdef defined(SAMD) + #define SerialPort SerialUSB +#else + #define SerialPort Serial +#endif MPU9250_DMP imu; diff --git a/examples/MPU9250_DMP_Tap/MPU9250_DMP_Tap.ino b/examples/MPU9250_DMP_Tap/MPU9250_DMP_Tap.ino index 48810a0..6ff4f48 100644 --- a/examples/MPU9250_DMP_Tap/MPU9250_DMP_Tap.ino +++ b/examples/MPU9250_DMP_Tap/MPU9250_DMP_Tap.ino @@ -21,7 +21,11 @@ Supported Platforms: *************************************************************/ #include -#define SerialPort SerialUSB +#ifdef defined(SAMD) + #define SerialPort SerialUSB +#else + #define SerialPort Serial +#endif MPU9250_DMP imu; diff --git a/examples/MPU9250_FIFO_Basic/MPU9250_FIFO_Basic.ino b/examples/MPU9250_FIFO_Basic/MPU9250_FIFO_Basic.ino index 3fd7c0d..b225b98 100644 --- a/examples/MPU9250_FIFO_Basic/MPU9250_FIFO_Basic.ino +++ b/examples/MPU9250_FIFO_Basic/MPU9250_FIFO_Basic.ino @@ -19,7 +19,11 @@ Supported Platforms: *************************************************************/ #include -#define SerialPort SerialUSB +#ifdef defined(SAMD) + #define SerialPort SerialUSB +#else + #define SerialPort Serial +#endif MPU9250_DMP imu; diff --git a/examples/MPU9250_Wake_On_Motion/MPU9250_Wake_On_Motion.ino b/examples/MPU9250_Wake_On_Motion/MPU9250_Wake_On_Motion.ino new file mode 100644 index 0000000..f6421c2 --- /dev/null +++ b/examples/MPU9250_Wake_On_Motion/MPU9250_Wake_On_Motion.ino @@ -0,0 +1,105 @@ +/************************************************************ +MPU9250_Wake_On_Motion + Wake-on-Motion interrupt sketch for MPU-9250 DMP Arduino Library +Oles Yaremenko @ home +original creation date: July 17, 2019 +https://github.com/sparkfun/SparkFun_MPU9250_DMP_Arduino_Library + +This example sketch demonstrates how to itialize the +MPU-9250 to implement Wake-On-Motion interrupt. + +Development environment specifics: +Arduino IDE 1.8.9 +ESP32 DevKit v.2 (interrupt on pin 4) +STM32L072CZ (interrupt on pin 4) +Supported Platforms: +- ATSAMD21 (Arduino Zero, SparkFun SAMD21 Breakouts) +- ESP32 Arduino Core +- STM32L0 with https://github.com/GrumpyOldPizza/ArduinoCore-stm32l0 +*************************************************************/ +#ifdef defined(SAMD) + #define SerialPort SerialUSB +#else + #define SerialPort Serial +#endif + +#include +#include +//Include the Invensense MPU9250 driver and DMP keys: +extern "C" { +#include "util/inv_mpu.h" +#include "util/inv_mpu_dmp_motion_driver.h" +} +// undef magnetometer if you use DMP only +#define MPU6500 // Used instead of #define MPU9250 to #undef AK8963_SECONDARY + +#define INTERRUPT_PIN 35 // Choose WOM interrupt pin + +static void imuISR(void); // Wake-on-Motion ISR + +volatile bool imuWoke; // ISR Woke-on-Motion Flag + +// Create an instance of the MPU9250_DMP class +MPU9250_DMP imu; + +void setup() +{ + pinMode(INTERRUPT_PIN, INPUT_PULLUP); + SerialPort.begin(115200); + SerialPort.println("MPU9250_Wake_On_Motion Example"); + + if (imu.begin() != INV_SUCCESS) + { + while (1) + { + SerialPort.println("Unable to communicate with MPU-9250"); + SerialPort.println("Check connections, and try again."); + SerialPort.println(); + delay(5000); + } + } + + // Power Down Gyro and Compass + imu.setSensors(INV_XYZ_ACCEL); + + // The interrupt level can either be active-high or low. Configure as active-low. + // Options are INT_ACTIVE_LOW or INT_ACTIVE_HIGH + imu.setIntLevel(INT_ACTIVE_LOW); + + // disable Auxiliary I2C Master I/F + mpu_set_bypass(0); + + // Enabe Wake On Motion low power mode with a threshold of 40 mg and + // an accelerometer data rate of 15.63 Hz. + // Only accelerometer is enabled in LP mode + // The interrupt is 50us pulse. + if (mpu_lp_motion_interrupt(40,0,2) != INV_SUCCESS) { + // Failed to initialize MPU-9250, report somehow + Serial.println(F("IMU set up failed. Please check installed IMU IC.")); + } + + // Attach Interupt Service Routine to be called on device motion (to check angles for example) + attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), imuISR, FALLING); + + + // Do not forget to detach interrupt if you stop WOM mode + //detachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN)); + imuWoke = false; +} + +void loop() +{ + // Check if the interrupt was fired + if ( imuWoke ) + { + // Do something. + SerialPort.println("Can't touch this!"); + // Reset WOM Flag + imuWoke = false; + } +} + +void imuISR() { + // Set Wake-on-Motion Flag only + imuWoke = true; +} \ No newline at end of file diff --git a/library.properties b/library.properties index 2498bf3..73c1bcf 100644 --- a/library.properties +++ b/library.properties @@ -6,4 +6,4 @@ sentence=Driver for InvenSense's MPU-9250 9-DOF IMU (3-axis gyroscope, 3-axis ac 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. category=Sensors url=https://github.com/sparkfun/MPU-9250_Breakout -architectures=samd +architectures=samd,esp8266,esp32,stm32l0 diff --git a/src/SparkFunMPU9250-DMP.cpp b/src/SparkFunMPU9250-DMP.cpp index db57b8f..114cee0 100644 --- a/src/SparkFunMPU9250-DMP.cpp +++ b/src/SparkFunMPU9250-DMP.cpp @@ -612,45 +612,39 @@ float MPU9250_DMP::calcQuat(long axis) float MPU9250_DMP::qToFloat(long number, unsigned char q) { - unsigned long mask = 0; - for (int i=0; i> q) + ((number & mask) / (float) (2<<(q-1))); + return (float)((double)number / (double)(1 << q)); } void MPU9250_DMP::computeEulerAngles(bool degrees) { - float dqw = qToFloat(qw, 30); - float dqx = qToFloat(qx, 30); - float dqy = qToFloat(qy, 30); - float dqz = qToFloat(qz, 30); - - float ysqr = dqy * dqy; - float t0 = -2.0f * (ysqr + dqz * dqz) + 1.0f; - float t1 = +2.0f * (dqx * dqy - dqw * dqz); - float t2 = -2.0f * (dqx * dqz + dqw * dqy); - float t3 = +2.0f * (dqy * dqz - dqw * dqx); - float t4 = -2.0f * (dqx * dqx + ysqr) + 1.0f; - - // Keep t2 within range of asin (-1, 1) - t2 = t2 > 1.0f ? 1.0f : t2; - t2 = t2 < -1.0f ? -1.0f : t2; - - pitch = asin(t2) * 2; - roll = atan2(t3, t4); - yaw = atan2(t1, t0); - - if (degrees) - { - pitch *= (180.0 / PI); - roll *= (180.0 / PI); - yaw *= (180.0 / PI); - if (pitch < 0) pitch = 360.0 + pitch; - if (roll < 0) roll = 360.0 + roll; - if (yaw < 0) yaw = 360.0 + yaw; - } + float dqw = qToFloat(qw, 30); + float dqx = qToFloat(qx, 30); + float dqy = qToFloat(qy, 30); + float dqz = qToFloat(qz, 30); + + float norm = sqrt(dqw*dqw + dqx*dqx + dqy*dqy + dqz*dqz); + dqw = dqw/norm; + dqx = dqx/norm; + dqy = dqy/norm; + dqz = dqz/norm; + + float ysqr = dqy * dqy; + + // roll (x-axis rotation) + float t0 = +2.0 * (dqw * dqx + dqy * dqz); + float t1 = +1.0 - 2.0 * (dqx * dqx + ysqr); + roll = atan2(t0, t1); + + // pitch (y-axis rotation) + float t2 = +2.0 * (dqw * dqy - dqz * dqx); + t2 = t2 > 1.0 ? 1.0 : t2; + t2 = t2 < -1.0 ? -1.0 : t2; + pitch = asin(t2); + + // yaw (z-axis rotation) + float t3 = +2.0 * (dqw * dqz + dqx * dqy); + float t4 = +1.0 - 2.0 * (ysqr + dqz * dqz); + yaw = atan2(t3, t4); } float MPU9250_DMP::computeCompassHeading(void) diff --git a/src/util/inv_mpu.c b/src/util/inv_mpu.c index 0410390..00de842 100644 --- a/src/util/inv_mpu.c +++ b/src/util/inv_mpu.c @@ -38,14 +38,20 @@ */ #include #define MPU9250 + #include "arduino_mpu9250_i2c.h" #include "arduino_mpu9250_clk.h" #define i2c_write(a, b, c, d) arduino_i2c_write(a, b, c, d) #define i2c_read(a, b, c, d) arduino_i2c_read(a, b, c, d) #define delay_ms arduino_delay_ms #define get_ms arduino_get_clock_ms -#define log_i _MLPrintLog -#define log_e _MLPrintLog +// Added by rupin to enable ESP32 compatibility +#if !defined (ESP32) + #define log_i _MLPrintLog + #define log_e _MLPrintLog +#else + #define min(a,b) a