Merge pull request #4581 from Staars/development

Add DMP-use to MPU-6050

lib/I2Cdevlib-MPU6050/MPU6050_6Axis_MotionApps20.h

@@ -46,7 +46,11 @@ THE SOFTWARE.
 // Tom Carpenter's conditional PROGMEM code
 // http://forum.arduino.cc/index.php?topic=129407.0
 #ifndef __arm__
-    #include <avr/pgmspace.h>
+    #if (defined(__AVR__))
+    #include <avr\pgmspace.h>
+    #else
+    #include <pgmspace.h>
+    #endif
 #else
     // Teensy 3.0 library conditional PROGMEM code from Paul Stoffregen
     #ifndef __PGMSPACE_H_
@@ -109,11 +113,19 @@ THE SOFTWARE.
     #define DEBUG_PRINTLN(x) Serial.println(x)
     #define DEBUG_PRINTLNF(x, y) Serial.println(x, y)
 #else
+#ifndef DEBUG_PRINT
     #define DEBUG_PRINT(x)
+#endif
+#ifndef DEBUG_PRINTF
     #define DEBUG_PRINTF(x, y)
+#endif
+#ifndef DEBUG_PRINTLN
     #define DEBUG_PRINTLN(x)
+#endif
+#ifndef DEBUG_PRINTLNF
     #define DEBUG_PRINTLNF(x, y)
 #endif
+#endif
 
 #define MPU6050_DMP_CODE_SIZE       1929    // dmpMemory[]
 #define MPU6050_DMP_CONFIG_SIZE     192     // dmpConfig[]
@@ -402,7 +414,7 @@ uint8_t MPU6050::dmpInitialize() {
             setIntEnabled(0x12);
 
             DEBUG_PRINTLN(F("Setting sample rate to 200Hz..."));
-            setRate(4); // 1khz / (1 + 4) = 200 Hz
+            setRate(1); // 1khz / (1 + 4) = 200 Hz
 
             DEBUG_PRINTLN(F("Setting external frame sync to TEMP_OUT_L[0]..."));
             setExternalFrameSync(MPU6050_EXT_SYNC_TEMP_OUT_L);

sonoff/my_user_config.h

@@ -313,7 +313,8 @@
 //    #define USE_PCA9685_FREQ 50                  // Define default PWM frequency in Hz to be used (must be within 24 to 1526) - If other value is used, it will rever to 50Hz
 //  #define USE_MPR121                             // Enable MPR121 controller (I2C addresses 0x5A, 0x5B, 0x5C and 0x5D) in input mode for touch buttons (+1k3 code)
 //  #define USE_CCS811                             // Enable CCS811 sensor (I2C address 0x5A) (+2k2 code)
-//  #define USE_MPU6050                            // Enable MPU6050 sensor (I2C address 0x68 AD0 low or 0x69 AD0 high) (+2k6 code)
+//  #define USE_MPU6050                            // Enable MPU6050 sensor (I2C address 0x68 AD0 low or 0x69 AD0 high) (+3K3 of code and 188 Bytes of RAM)
+//    #define USE_MPU6050_DMP                      // Enable in MPU6050 to use the DMP on the chip, should create better results (+8k6 of code)
 //  #define USE_DS3231                             // Enable DS3231 external RTC in case no Wifi is avaliable. See docs in the source file (+1k2 code)
 //    #define USE_RTC_ADDR  0x68                   // Default I2C address 0x68
 //  #define USE_MGC3130                            // Enable MGC3130 Electric Field Effect Sensor (I2C address 0x42) (+2k7 code, 0k3 mem)

sonoff/xsns_32_mpu6050.ino

@@ -42,11 +42,49 @@ int16_t MPU_6050_ax = 0, MPU_6050_ay = 0, MPU_6050_az = 0;
 int16_t MPU_6050_gx = 0, MPU_6050_gy = 0, MPU_6050_gz = 0;
 int16_t MPU_6050_temperature = 0;
 
-#include <MPU6050.h>
+#ifdef USE_MPU6050_DMP
+  #include "MPU6050_6Axis_MotionApps20.h"
+  #include "I2Cdev.h"
+  #include <helper_3dmath.h>
+  typedef struct MPU6050_DMP{
+  uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
+  uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
+  uint16_t fifoCount;     // count of all bytes currently in FIFO
+  uint8_t fifoBuffer[64]; // FIFO storage buffer
+  Quaternion q;           // [w, x, y, z]         quaternion container
+  VectorInt16 aa;         // [x, y, z]            accel sensor measurements
+  VectorInt16 aaReal;     // [x, y, z]            gravity-free accel sensor measurements
+  VectorFloat gravity;    // [x, y, z]            gravity vector
+  float euler[3];         // [psi, theta, phi]    Euler angle container
+  } MPU6050_DMP;
+
+  MPU6050_DMP MPU6050_dmp;
+#else
+  #include <MPU6050.h>
+#endif //USE_MPU6050_DMP
 MPU6050 mpu6050;
 
 void MPU_6050PerformReading(void)
 {
+#ifdef USE_MPU6050_DMP
+    mpu6050.resetFIFO(); // with a default dampling rate of 200Hz, we create a delay of approx. 5ms with a complete read cycle
+    MPU6050_dmp.fifoCount = mpu6050.getFIFOCount();
+    while (MPU6050_dmp.fifoCount < MPU6050_dmp.packetSize) MPU6050_dmp.fifoCount = mpu6050.getFIFOCount();
+    mpu6050.getFIFOBytes(MPU6050_dmp.fifoBuffer, MPU6050_dmp.packetSize);
+    MPU6050_dmp.fifoCount -= MPU6050_dmp.packetSize;
+    // calculate euler and acceleration with the DMP
+    mpu6050.dmpGetQuaternion(&MPU6050_dmp.q, MPU6050_dmp.fifoBuffer);
+    mpu6050.dmpGetEuler(MPU6050_dmp.euler, &MPU6050_dmp.q);
+    mpu6050.dmpGetAccel(&MPU6050_dmp.aa, MPU6050_dmp.fifoBuffer);
+    mpu6050.dmpGetGravity(&MPU6050_dmp.gravity, &MPU6050_dmp.q);
+    mpu6050.dmpGetLinearAccel(&MPU6050_dmp.aaReal, &MPU6050_dmp.aa, &MPU6050_dmp.gravity);
+    MPU_6050_gx = MPU6050_dmp.euler[0] * 180/M_PI;
+    MPU_6050_gy = MPU6050_dmp.euler[1] * 180/M_PI;
+    MPU_6050_gz = MPU6050_dmp.euler[2] * 180/M_PI;
+    MPU_6050_ax = MPU6050_dmp.aaReal.x;
+    MPU_6050_ay = MPU6050_dmp.aaReal.y;
+    MPU_6050_az = MPU6050_dmp.aaReal.z;
+#else
   mpu6050.getMotion6(
     &MPU_6050_ax,
     &MPU_6050_ay,
@@ -55,7 +93,7 @@ void MPU_6050PerformReading(void)
     &MPU_6050_gy,
     &MPU_6050_gz
   );
-
+#endif //USE_MPU6050_DMP
   MPU_6050_temperature = mpu6050.getTemperature();
 }
 
@@ -84,14 +122,30 @@ void MPU_6050Detect(void)
 
   for (byte i = 0; i < sizeof(MPU_6050_addresses); i++)
   {
+    if(!I2cDevice(MPU_6050_addresses[i]))
+      {
+        break;
+      }
     MPU_6050_address = MPU_6050_addresses[i];
+    mpu6050.setAddr(MPU_6050_addresses[i]);
 
-    mpu6050.setAddr(MPU_6050_address);
+#ifdef USE_MPU6050_DMP
+    MPU6050_dmp.devStatus = mpu6050.dmpInitialize();
+    mpu6050.setXGyroOffset(220);
+    mpu6050.setYGyroOffset(76);
+    mpu6050.setZGyroOffset(-85);
+    mpu6050.setZAccelOffset(1788); 
+    if (MPU6050_dmp.devStatus == 0) {
+      mpu6050.setDMPEnabled(true);
+      MPU6050_dmp.packetSize = mpu6050.dmpGetFIFOPacketSize();
+      MPU_6050_found = true;
+    } 
+#else
     mpu6050.initialize();
-
+    MPU_6050_found = mpu6050.testConnection();
+#endif //USE_MPU6050_DMP
     Settings.flag2.axis_resolution = 2;  // Need to be services by command Sensor32
 
-    MPU_6050_found = mpu6050.testConnection();
   }
 
   if (MPU_6050_found)
@@ -119,11 +173,10 @@ const char HTTP_SNS_GZ_AXIS[] PROGMEM = "%s{s}%s " D_GZ_AXIS "{m}%s{e}";
 
 void MPU_6050Show(boolean json)
 {
-  double tempConv = (MPU_6050_temperature / 340.0 + 35.53);
-
   if (MPU_6050_found) {
     MPU_6050PerformReading();
 
+    double tempConv = (MPU_6050_temperature / 340.0 + 35.53);
     char temperature[10];
     dtostrfd(tempConv, Settings.flag2.temperature_resolution, temperature);
     char axis_ax[10];
@@ -140,22 +193,22 @@ void MPU_6050Show(boolean json)
     dtostrfd(MPU_6050_gz, Settings.flag2.axis_resolution, axis_gz);
 
     if (json) {
-      char json_axis_ax[40];
+      char json_axis_ax[25];
       snprintf_P(json_axis_ax, sizeof(json_axis_ax), PSTR(",\"" D_JSON_AXIS_AX "\":%s"), axis_ax);
-      char json_axis_ay[40];
+      char json_axis_ay[25];
       snprintf_P(json_axis_ay, sizeof(json_axis_ay), PSTR(",\"" D_JSON_AXIS_AY "\":%s"), axis_ay);
-      char json_axis_az[40];
+      char json_axis_az[25];
       snprintf_P(json_axis_az, sizeof(json_axis_az), PSTR(",\"" D_JSON_AXIS_AZ "\":%s"), axis_az);
-      char json_axis_gx[40];
+      char json_axis_gx[25];
       snprintf_P(json_axis_gx, sizeof(json_axis_gx), PSTR(",\"" D_JSON_AXIS_GX "\":%s"), axis_gx);
-      char json_axis_gy[40];
+      char json_axis_gy[25];
       snprintf_P(json_axis_gy, sizeof(json_axis_gy), PSTR(",\"" D_JSON_AXIS_GY "\":%s"), axis_gy);
-      char json_axis_gz[40];
+      char json_axis_gz[25];
       snprintf_P(json_axis_gz, sizeof(json_axis_gz), PSTR(",\"" D_JSON_AXIS_GZ "\":%s"), axis_gz);
       snprintf_P(mqtt_data, sizeof(mqtt_data), PSTR("%s,\"%s\":{\"" D_JSON_TEMPERATURE "\":%s%s%s%s%s%s%s}"),
-        mqtt_data, D_SENSOR_MPU6050, temperature, json_axis_ax, json_axis_ay, json_axis_az, json_axis_gx, json_axis_gy, json_axis_gz);
+      mqtt_data, D_SENSOR_MPU6050, temperature, json_axis_ax, json_axis_ay, json_axis_az, json_axis_gx, json_axis_gy, json_axis_gz);
 #ifdef USE_DOMOTICZ
-      DomoticzTempHumSensor(temperature, 0);
+      DomoticzSensor(DZ_TEMP, temperature);
 #endif // USE_DOMOTICZ
 #ifdef USE_WEBSERVER
     } else {