Wake-On-Motion Example Added and community fixes

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.
 

examples/MPU9250_Basic/MPU9250_Basic.ino

@@ -17,7 +17,11 @@ Supported Platforms:
 *************************************************************/
 #include <SparkFunMPU9250-DMP.h>
 
-#define SerialPort SerialUSB
+#ifdef defined(SAMD)
+ #define SerialPort SerialUSB
+#else
+  #define SerialPort Serial
+#endif
 
 MPU9250_DMP imu;
 

examples/MPU9250_Basic_Interrupt/MPU9250_Basic_Interrupt.ino

@@ -19,7 +19,12 @@ Supported Platforms:
 *************************************************************/
 #include <SparkFunMPU9250-DMP.h>
 
-#define SerialPort SerialUSB
+#ifdef defined(SAMD)
+ #define SerialPort SerialUSB
+#else
+  #define SerialPort Serial
+#endif
+
 #define INTERRUPT_PIN 4
 
 MPU9250_DMP imu;

examples/MPU9250_DMP_Gyro_Cal/MPU9250_DMP_Gyro_Cal.ino

@@ -19,7 +19,11 @@ Supported Platforms:
 *************************************************************/
 #include <SparkFunMPU9250-DMP.h>
 
-#define SerialPort SerialUSB
+#ifdef defined(SAMD)
+ #define SerialPort SerialUSB
+#else
+  #define SerialPort Serial
+#endif
 
 MPU9250_DMP imu;
 

examples/MPU9250_DMP_Orientation/MPU9250_DMP_Orientation.ino

@@ -17,7 +17,11 @@ Supported Platforms:
 *************************************************************/
 #include <SparkFunMPU9250-DMP.h>
 
-#define SerialPort SerialUSB
+#ifdef defined(SAMD)
+ #define SerialPort SerialUSB
+#else
+  #define SerialPort Serial
+#endif
 
 MPU9250_DMP imu;
 

examples/MPU9250_DMP_Pedometer/MPU9250_DMP_Pedometer.ino

@@ -20,7 +20,11 @@ Supported Platforms:
 *************************************************************/
 #include <SparkFunMPU9250-DMP.h>
 
-#define SerialPort SerialUSB
+#ifdef defined(SAMD)
+ #define SerialPort SerialUSB
+#else
+  #define SerialPort Serial
+#endif
 
 MPU9250_DMP imu;
 

examples/MPU9250_DMP_Quaternion/MPU9250_DMP_Quaternion.ino

@@ -23,7 +23,11 @@ Supported Platforms:
 *************************************************************/
 #include <SparkFunMPU9250-DMP.h>
 
-#define SerialPort SerialUSB
+#ifdef defined(SAMD)
+ #define SerialPort SerialUSB
+#else
+  #define SerialPort Serial
+#endif
 
 MPU9250_DMP imu;
 

examples/MPU9250_DMP_Tap/MPU9250_DMP_Tap.ino

@@ -21,7 +21,11 @@ Supported Platforms:
 *************************************************************/
 #include <SparkFunMPU9250-DMP.h>
 
-#define SerialPort SerialUSB
+#ifdef defined(SAMD)
+ #define SerialPort SerialUSB
+#else
+  #define SerialPort Serial
+#endif
 
 MPU9250_DMP imu;
 

examples/MPU9250_FIFO_Basic/MPU9250_FIFO_Basic.ino

@@ -19,7 +19,11 @@ Supported Platforms:
 *************************************************************/
 #include <SparkFunMPU9250-DMP.h>
 
-#define SerialPort SerialUSB
+#ifdef defined(SAMD)
+ #define SerialPort SerialUSB
+#else
+  #define SerialPort Serial
+#endif
 
 MPU9250_DMP imu;
 

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 <SparkFunMPU9250-DMP.h>
+#include <MPU9250_RegisterMap.h>
+//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;
+}

library.properties

@@ -6,4 +6,4 @@ sentence=Driver for InvenSense's MPU-9250 9-DOF IMU (3-axis gyroscope, 3-axis ac
 paragraph=The <a href="https://www.invensense.com/products/motion-tracking/9-axis/mpu-9250/">MPU-9250</a> 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 I<sup>2</sup>C 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

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; i++)
-	{
-		mask |= (1<<i);
-	}
-	return (number >> 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)

src/util/inv_mpu.c

@@ -38,14 +38,20 @@
  */
 #include <Arduino.h>
 #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<b ? a : b
+#endif
 static inline int reg_int_cb(struct int_param_s *int_param)
 {
 
@@ -433,7 +439,7 @@ const struct gyro_reg_s reg = {
 #endif
 };
 const struct hw_s hw = {
-    .addr           = 0x68,
+    .addr           = MPU_I2C_ADR,
     .max_fifo       = 1024,
     .num_reg        = 118,
     .temp_sens      = 340,
@@ -513,7 +519,7 @@ const struct gyro_reg_s reg = {
 #endif
 };
 const struct hw_s hw = {
-    .addr           = 0x68,
+    .addr           = MPU_I2C_ADR,
     .max_fifo       = 1024,
     .num_reg        = 128,
     .temp_sens      = 321,