From 140aa8780ac73d4b216fd731416238fe74478ae0 Mon Sep 17 00:00:00 2001 From: Staars Date: Sun, 9 Dec 2018 09:34:22 +0100 Subject: [PATCH 1/7] add DMP mode to MPU-6050 --- sonoff/xsns_32_mpu6050.ino | 82 +++++++++++++++++++++++++++++++------- 1 file changed, 68 insertions(+), 14 deletions(-) diff --git a/sonoff/xsns_32_mpu6050.ino b/sonoff/xsns_32_mpu6050.ino index 0c6f50f09..7115800dd 100644 --- a/sonoff/xsns_32_mpu6050.ino +++ b/sonoff/xsns_32_mpu6050.ino @@ -19,6 +19,7 @@ #ifdef USE_I2C #ifdef USE_MPU6050 +#define USE_MPU6050_DMP // Use the DMP on the chip, should create better results /*********************************************************************************************\ * MPU6050 3 axis gyroscope and temperature sensor * @@ -42,11 +43,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 +#ifdef USE_MPU6050_DMP + #include "MPU6050_6Axis_MotionApps20.h" + #include "I2Cdev.h" + #include + 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 +#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 +94,7 @@ void MPU_6050PerformReading(void) &MPU_6050_gy, &MPU_6050_gz ); - +#endif //USE_MPU6050_DMP MPU_6050_temperature = mpu6050.getTemperature(); } @@ -84,14 +123,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 +174,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,20 +194,20 @@ 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); #endif // USE_DOMOTICZ From aab716798a2abe1c83bac0b6eab2b0f4f6094970 Mon Sep 17 00:00:00 2001 From: Staars Date: Sun, 9 Dec 2018 09:35:23 +0100 Subject: [PATCH 2/7] fix compile issue on ESP8266 --- lib/I2Cdevlib-MPU6050/MPU6050_6Axis_MotionApps20.h | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/lib/I2Cdevlib-MPU6050/MPU6050_6Axis_MotionApps20.h b/lib/I2Cdevlib-MPU6050/MPU6050_6Axis_MotionApps20.h index 1f7b88589..46920438a 100644 --- a/lib/I2Cdevlib-MPU6050/MPU6050_6Axis_MotionApps20.h +++ b/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 + #if (defined(__AVR__)) + #include + #else + #include + #endif #else // Teensy 3.0 library conditional PROGMEM code from Paul Stoffregen #ifndef __PGMSPACE_H_ @@ -402,7 +406,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); From 4fb6773ffbed6a1f6f00cf8cc76e913a8320d5f6 Mon Sep 17 00:00:00 2001 From: Adrian Scillato <39969427+ascillato2@users.noreply.github.com> Date: Mon, 10 Dec 2018 19:28:22 +0000 Subject: [PATCH 3/7] Update my_user_config.h --- sonoff/my_user_config.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sonoff/my_user_config.h b/sonoff/my_user_config.h index ef81066f3..cade32e94 100644 --- a/sonoff/my_user_config.h +++ b/sonoff/my_user_config.h @@ -312,7 +312,7 @@ // #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) (+12KB of code and 188 Bytes RAM) // #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) From a75bc93634dd54a8b6b2b3f0b3c05394e2c6aaf0 Mon Sep 17 00:00:00 2001 From: Adrian Scillato <39969427+ascillato2@users.noreply.github.com> Date: Mon, 10 Dec 2018 19:43:19 +0000 Subject: [PATCH 4/7] Fix Warning in Platformio if building MPU and KNX together --- lib/I2Cdevlib-MPU6050/MPU6050_6Axis_MotionApps20.h | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/lib/I2Cdevlib-MPU6050/MPU6050_6Axis_MotionApps20.h b/lib/I2Cdevlib-MPU6050/MPU6050_6Axis_MotionApps20.h index 46920438a..90704f0c7 100644 --- a/lib/I2Cdevlib-MPU6050/MPU6050_6Axis_MotionApps20.h +++ b/lib/I2Cdevlib-MPU6050/MPU6050_6Axis_MotionApps20.h @@ -113,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[] From 384cc5cb16838e4845a4ade9bf792a117eafd716 Mon Sep 17 00:00:00 2001 From: Adrian Scillato <39969427+ascillato2@users.noreply.github.com> Date: Mon, 10 Dec 2018 20:03:36 +0000 Subject: [PATCH 5/7] USE_MPU6050_DMP moved to my_user_config.h --- sonoff/my_user_config.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/sonoff/my_user_config.h b/sonoff/my_user_config.h index cade32e94..048519bb2 100644 --- a/sonoff/my_user_config.h +++ b/sonoff/my_user_config.h @@ -312,7 +312,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) (+12KB of code and 188 Bytes RAM) +// #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) From e5d3c3b793680e16cde728a149c2bc7efc339955 Mon Sep 17 00:00:00 2001 From: Adrian Scillato <39969427+ascillato2@users.noreply.github.com> Date: Mon, 10 Dec 2018 20:04:48 +0000 Subject: [PATCH 6/7] USE_MPU6050_DMP moved to my_user_config.h --- sonoff/xsns_32_mpu6050.ino | 1 - 1 file changed, 1 deletion(-) diff --git a/sonoff/xsns_32_mpu6050.ino b/sonoff/xsns_32_mpu6050.ino index 7115800dd..6a6d5a1df 100644 --- a/sonoff/xsns_32_mpu6050.ino +++ b/sonoff/xsns_32_mpu6050.ino @@ -19,7 +19,6 @@ #ifdef USE_I2C #ifdef USE_MPU6050 -#define USE_MPU6050_DMP // Use the DMP on the chip, should create better results /*********************************************************************************************\ * MPU6050 3 axis gyroscope and temperature sensor * From bc30a18adee249703847a0a4a143dd5cea430dc2 Mon Sep 17 00:00:00 2001 From: Adrian Scillato <39969427+ascillato2@users.noreply.github.com> Date: Tue, 11 Dec 2018 00:57:41 +0000 Subject: [PATCH 7/7] Corrected Domoticz Temp published from MPU6050 --- sonoff/xsns_32_mpu6050.ino | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sonoff/xsns_32_mpu6050.ino b/sonoff/xsns_32_mpu6050.ino index 6a6d5a1df..b80366194 100644 --- a/sonoff/xsns_32_mpu6050.ino +++ b/sonoff/xsns_32_mpu6050.ino @@ -208,7 +208,7 @@ void MPU_6050Show(boolean json) 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); #ifdef USE_DOMOTICZ - DomoticzTempHumSensor(temperature, 0); + DomoticzSensor(DZ_TEMP, temperature); #endif // USE_DOMOTICZ #ifdef USE_WEBSERVER } else {