Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Add DMP-use to MPU-6050 #4581

Merged
merged 7 commits into from
Dec 11, 2018
Merged

Add DMP-use to MPU-6050 #4581

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
140aa87
add DMP mode to MPU-6050
Staars Dec 9, 2018
aab7167
fix compile issue on ESP8266
Staars Dec 9, 2018
4fb6773
Update my_user_config.h
ascillato2 Dec 10, 2018
a75bc93
Fix Warning in Platformio if building MPU and KNX together
ascillato2 Dec 10, 2018
384cc5c
USE_MPU6050_DMP moved to my_user_config.h
ascillato2 Dec 10, 2018
e5d3c3b
USE_MPU6050_DMP moved to my_user_config.h
ascillato2 Dec 10, 2018
bc30a18
Corrected Domoticz Temp published from MPU6050
ascillato2 Dec 11, 2018
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
16 changes: 14 additions & 2 deletions lib/I2Cdevlib-MPU6050/MPU6050_6Axis_MotionApps20.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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_
Expand Down Expand Up @@ -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[]
Expand Down Expand Up @@ -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);
Expand Down
3 changes: 2 additions & 1 deletion sonoff/my_user_config.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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) (+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)
Expand Down
83 changes: 68 additions & 15 deletions sonoff/xsns_32_mpu6050.ino
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand All @@ -55,7 +93,7 @@ void MPU_6050PerformReading(void)
&MPU_6050_gy,
&MPU_6050_gz
);

#endif //USE_MPU6050_DMP
MPU_6050_temperature = mpu6050.getTemperature();
}

Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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];
Expand All @@ -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 {
Expand Down