diff --git a/lib/I2Cdevlib-MPU6050/MPU6050_6Axis_MotionApps20.h b/lib/I2Cdevlib-MPU6050/MPU6050_6Axis_MotionApps20.h index 1f7b88589..90704f0c7 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_ @@ -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); diff --git a/sonoff/my_user_config.h b/sonoff/my_user_config.h index 043a81111..b34fc0099 100644 --- a/sonoff/my_user_config.h +++ b/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) diff --git a/sonoff/xsns_32_mpu6050.ino b/sonoff/xsns_32_mpu6050.ino index 0c6f50f09..b80366194 100644 --- a/sonoff/xsns_32_mpu6050.ino +++ b/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 +#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 +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 {