Skip to content

Add DMP-use to MPU-6050 #4581

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

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 2 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.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
8 changes: 6 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 @@ -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);
Expand Down
82 changes: 68 additions & 14 deletions sonoff/xsns_32_mpu6050.ino
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
*
Expand All @@ -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 <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 +94,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 +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)
Expand Down Expand Up @@ -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];
Expand All @@ -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
Expand Down