Skip to content

Commit

Permalink
Merge pull request #4581 from Staars/development
Browse files Browse the repository at this point in the history 
Add DMP-use to MPU-6050
  • Loading branch information
@arendst
arendst committed Dec 11, 2018
2 parents 57c2e01 + bc30a18 commit 400529c
Show file tree
Hide file tree
Showing 3 changed files with 84 additions and 18 deletions.
16 changes: 14 additions & 2 deletions lib/I2Cdevlib-MPU6050/MPU6050_6Axis_MotionApps20.h
View file
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
Expand Up @@ -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)
Expand Down
83 changes: 68 additions & 15 deletions sonoff/xsns_32_mpu6050.ino
View file
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

0 comments on commit 400529c

Please sign in to comment.