From 7f42caa6c0f9c139b8a8f1b3acd61422834609ba Mon Sep 17 00:00:00 2001 From: Philip Barclay Date: Mon, 18 Nov 2019 19:10:45 +1300 Subject: [PATCH 1/3] Added YPR to web interface --- sonoff/xsns_32_mpu6050.ino | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/sonoff/xsns_32_mpu6050.ino b/sonoff/xsns_32_mpu6050.ino index 58915efc9..d212a45e2 100644 --- a/sonoff/xsns_32_mpu6050.ino +++ b/sonoff/xsns_32_mpu6050.ino @@ -56,6 +56,7 @@ int16_t MPU_6050_temperature = 0; 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 + float yawPitchRoll[3]; // [yaw, pitch roll] Yaw-pitch-roll container } MPU6050_DMP; MPU6050_DMP MPU6050_dmp; @@ -78,6 +79,7 @@ void MPU_6050PerformReading(void) 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); + mpu6050.dmpGetYawPitchRoll(MPU6050_dmp.yawPitchRoll, &MPU6050_dmp.q, &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; @@ -161,7 +163,13 @@ const char HTTP_SNS_AXIS[] PROGMEM = "{s}" D_SENSOR_MPU6050 " " D_AZ_AXIS "{m}%s{e}" // {s} = , {m} = , {e} = "{s}" D_SENSOR_MPU6050 " " D_GX_AXIS "{m}%s{e}" // {s} = , {m} = , {e} = "{s}" D_SENSOR_MPU6050 " " D_GY_AXIS "{m}%s{e}" // {s} = , {m} = , {e} = - "{s}" D_SENSOR_MPU6050 " " D_GZ_AXIS "{m}%s{e}"; // {s} = , {m} = , {e} = + "{s}" D_SENSOR_MPU6050 " " D_GZ_AXIS "{m}%s{e}" // {s} = , {m} = , {e} = +#ifdef USE_MPU6050_DMP + "{s}" D_SENSOR_MPU6050 " " D_YAW "{m}%s{e}" // {s} = , {m} = , {e} = + "{s}" D_SENSOR_MPU6050 " " D_PITCH "{m}%s{e}" // {s} = , {m} = , {e} = + "{s}" D_SENSOR_MPU6050 " " D_ROLL "{m}%s{e}" // {s} = , {m} = , {e} = +#endif // USE_MPU_DMP + ; #endif // USE_WEBSERVER #define D_JSON_AXIS_AX "AccelXAxis" From dac6ce1e54b73eda46da6d22b8058550184a393d Mon Sep 17 00:00:00 2001 From: Philip Barclay Date: Mon, 18 Nov 2019 21:33:34 +1300 Subject: [PATCH 2/3] Added YPR results to webserver and JSON output if DMP enabled. --- tasmota/xsns_32_mpu6050.ino | 32 ++++++++++++++++++++++++++------ 1 file changed, 26 insertions(+), 6 deletions(-) diff --git a/tasmota/xsns_32_mpu6050.ino b/tasmota/xsns_32_mpu6050.ino index 7b26101a1..6f3cff5e8 100644 --- a/tasmota/xsns_32_mpu6050.ino +++ b/tasmota/xsns_32_mpu6050.ino @@ -69,7 +69,7 @@ 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.resetFIFO(); // with a default sampling 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); @@ -147,6 +147,10 @@ void MPU_6050Detect(void) } } +#define D_YAW "Yaw" +#define D_PITCH "Pitch" +#define D_ROLL "Roll" + #ifdef USE_WEBSERVER const char HTTP_SNS_AXIS[] PROGMEM = "{s}" D_SENSOR_MPU6050 " " D_AX_AXIS "{m}%s{e}" // {s} = , {m} = , {e} = @@ -154,13 +158,13 @@ const char HTTP_SNS_AXIS[] PROGMEM = "{s}" D_SENSOR_MPU6050 " " D_AZ_AXIS "{m}%s{e}" // {s} = , {m} = , {e} = "{s}" D_SENSOR_MPU6050 " " D_GX_AXIS "{m}%s{e}" // {s} = , {m} = , {e} = "{s}" D_SENSOR_MPU6050 " " D_GY_AXIS "{m}%s{e}" // {s} = , {m} = , {e} = - "{s}" D_SENSOR_MPU6050 " " D_GZ_AXIS "{m}%s{e}" // {s} = , {m} = , {e} = + "{s}" D_SENSOR_MPU6050 " " D_GZ_AXIS "{m}%s{e}"; // {s} = , {m} = , {e} = #ifdef USE_MPU6050_DMP +const char HTTP_SNS_YPR[] PROGMEM = "{s}" D_SENSOR_MPU6050 " " D_YAW "{m}%s{e}" // {s} = , {m} = , {e} = - "{s}" D_SENSOR_MPU6050 " " D_PITCH "{m}%s{e}" // {s} = , {m} = , {e} = - "{s}" D_SENSOR_MPU6050 " " D_ROLL "{m}%s{e}" // {s} = , {m} = , {e} = -#endif // USE_MPU_DMP - ; + "{s}" D_SENSOR_MPU6050 " " D_PITCH "{m}%s{e}" // {s} = , {m} = , {e} = + "{s}" D_SENSOR_MPU6050 " " D_ROLL "{m}%s{e}"; // {s} = , {m} = , {e} = +#endif // USE_MPU6050_DMP #endif // USE_WEBSERVER #define D_JSON_AXIS_AX "AccelXAxis" @@ -169,6 +173,9 @@ const char HTTP_SNS_AXIS[] PROGMEM = #define D_JSON_AXIS_GX "GyroXAxis" #define D_JSON_AXIS_GY "GyroYAxis" #define D_JSON_AXIS_GZ "GyroZAxis" +#define D_JSON_YAW "Yaw" +#define D_JSON_PITCH "Pitch" +#define D_JSON_ROLL "Roll" void MPU_6050Show(bool json) { @@ -203,8 +210,20 @@ void MPU_6050Show(bool json) snprintf_P(json_axis_gy, sizeof(json_axis_gy), PSTR(",\"" D_JSON_AXIS_GY "\":%s"), axis_gy); char json_axis_gz[25]; snprintf_P(json_axis_gz, sizeof(json_axis_gz), PSTR(",\"" D_JSON_AXIS_GZ "\":%s"), axis_gz); +#ifdef USE_MPU6050_DMP + char json_ypr_y[25]; + snprintf_P(json_ypr_y, sizeof(json_ypr_y), PSTR(",\"" D_JSON_YAW "\":%s"), MPU6050_dmp.yawPitchRoll[0]); + char json_ypr_p[25]; + snprintf_P(json_ypr_p, sizeof(json_ypr_p), PSTR(",\"" D_JSON_PITCH "\":%s"), MPU6050_dmp.yawPitchRoll[1]); + char json_ypr_r[25]; + snprintf_P(json_ypr_r, sizeof(json_ypr_r), PSTR(",\"" D_JSON_ROLL "\":%s"), MPU6050_dmp.yawPitchRoll[2]); + ResponseAppend_P(PSTR(",\"%s\":{\"" D_JSON_TEMPERATURE "\":%s%s%s%s%s%s%s%s%s%s}"), + D_SENSOR_MPU6050, temperature, json_axis_ax, json_axis_ay, json_axis_az, json_axis_gx, json_axis_gy, json_axis_gz, + json_ypr_y, json_ypr_p, json_ypr_r); +#else ResponseAppend_P(PSTR(",\"%s\":{\"" D_JSON_TEMPERATURE "\":%s%s%s%s%s%s%s}"), D_SENSOR_MPU6050, temperature, json_axis_ax, json_axis_ay, json_axis_az, json_axis_gx, json_axis_gy, json_axis_gz); +#endif // USE_MPU6050_DMP #ifdef USE_DOMOTICZ DomoticzSensor(DZ_TEMP, temperature); #endif // USE_DOMOTICZ @@ -212,6 +231,7 @@ void MPU_6050Show(bool json) } else { WSContentSend_PD(HTTP_SNS_TEMP, D_SENSOR_MPU6050, temperature, TempUnit()); WSContentSend_PD(HTTP_SNS_AXIS, axis_ax, axis_ay, axis_az, axis_gx, axis_gy, axis_gz); + WSContentSend_PD(HTTP_SNS_YPR, MPU6050_dmp.yawPitchRoll[0], MPU6050_dmp.yawPitchRoll[1], MPU6050_dmp.yawPitchRoll[2]); #endif // USE_WEBSERVER } } From fe63e5cca2fcae187e7d6c3e3211db0414667821 Mon Sep 17 00:00:00 2001 From: Philip Barclay Date: Fri, 29 Nov 2019 21:07:39 +1300 Subject: [PATCH 3/3] Stringification and conversion from radians to degrees. --- tasmota/xsns_32_mpu6050.ino | 18 ++++++++++++++---- 1 file changed, 14 insertions(+), 4 deletions(-) diff --git a/tasmota/xsns_32_mpu6050.ino b/tasmota/xsns_32_mpu6050.ino index 6f3cff5e8..bcec100b0 100644 --- a/tasmota/xsns_32_mpu6050.ino +++ b/tasmota/xsns_32_mpu6050.ino @@ -196,6 +196,14 @@ void MPU_6050Show(bool json) dtostrfd(MPU_6050_gy, Settings.flag2.axis_resolution, axis_gy); char axis_gz[33]; dtostrfd(MPU_6050_gz, Settings.flag2.axis_resolution, axis_gz); +#ifdef USE_MPU6050_DMP + char axis_yaw[33]; + dtostrfd(MPU6050_dmp.yawPitchRoll[0] / PI * 180.0, Settings.flag2.axis_resolution, axis_yaw); + char axis_pitch[33]; + dtostrfd(MPU6050_dmp.yawPitchRoll[1] / PI * 180.0, Settings.flag2.axis_resolution, axis_pitch); + char axis_roll[33]; + dtostrfd(MPU6050_dmp.yawPitchRoll[2] / PI * 180.0, Settings.flag2.axis_resolution, axis_roll); +#endif // USE_MPU6050_DMP if (json) { char json_axis_ax[25]; @@ -212,11 +220,11 @@ void MPU_6050Show(bool json) snprintf_P(json_axis_gz, sizeof(json_axis_gz), PSTR(",\"" D_JSON_AXIS_GZ "\":%s"), axis_gz); #ifdef USE_MPU6050_DMP char json_ypr_y[25]; - snprintf_P(json_ypr_y, sizeof(json_ypr_y), PSTR(",\"" D_JSON_YAW "\":%s"), MPU6050_dmp.yawPitchRoll[0]); + snprintf_P(json_ypr_y, sizeof(json_ypr_y), PSTR(",\"" D_JSON_YAW "\":%s"), axis_yaw); char json_ypr_p[25]; - snprintf_P(json_ypr_p, sizeof(json_ypr_p), PSTR(",\"" D_JSON_PITCH "\":%s"), MPU6050_dmp.yawPitchRoll[1]); + snprintf_P(json_ypr_p, sizeof(json_ypr_p), PSTR(",\"" D_JSON_PITCH "\":%s"), axis_pitch); char json_ypr_r[25]; - snprintf_P(json_ypr_r, sizeof(json_ypr_r), PSTR(",\"" D_JSON_ROLL "\":%s"), MPU6050_dmp.yawPitchRoll[2]); + snprintf_P(json_ypr_r, sizeof(json_ypr_r), PSTR(",\"" D_JSON_ROLL "\":%s"), axis_roll); ResponseAppend_P(PSTR(",\"%s\":{\"" D_JSON_TEMPERATURE "\":%s%s%s%s%s%s%s%s%s%s}"), D_SENSOR_MPU6050, temperature, json_axis_ax, json_axis_ay, json_axis_az, json_axis_gx, json_axis_gy, json_axis_gz, json_ypr_y, json_ypr_p, json_ypr_r); @@ -231,7 +239,9 @@ void MPU_6050Show(bool json) } else { WSContentSend_PD(HTTP_SNS_TEMP, D_SENSOR_MPU6050, temperature, TempUnit()); WSContentSend_PD(HTTP_SNS_AXIS, axis_ax, axis_ay, axis_az, axis_gx, axis_gy, axis_gz); - WSContentSend_PD(HTTP_SNS_YPR, MPU6050_dmp.yawPitchRoll[0], MPU6050_dmp.yawPitchRoll[1], MPU6050_dmp.yawPitchRoll[2]); +#ifdef USE_MPU6050_DMP + WSContentSend_PD(HTTP_SNS_YPR, axis_yaw, axis_pitch, axis_roll); +#endif // USE_MPU6050_DMP #endif // USE_WEBSERVER } }