diff --git a/platformio.ini b/platformio.ini index 00029787..4edc214e 100644 --- a/platformio.ini +++ b/platformio.ini @@ -36,11 +36,17 @@ lib_deps = [STARBASE_USERMOD_HA] build_flags = - -D STARMOD_USERMOD_HA + -D STARBASE_USERMOD_HA -D ARDUINOHA_DEBUG lib_deps = https://github.com/dawidchyrzynski/arduino-home-assistant.git#2.1.0 +[STARBASE_USERMOD_MPU6050] +build_flags = + -D STARBASE_USERMOD_MPU6050 +lib_deps = + ElectronicCats/MPU6050 @ 1.3.0 + [STARBASE] build_flags = @@ -52,12 +58,14 @@ build_flags = -D STARBASE_DEVMODE ${ESPAsyncWebServer.build_flags} ${STARBASE_USERMOD_E131.build_flags} + ${STARBASE_USERMOD_MPU6050.build_flags} ; ${STARBASE_USERMOD_HA.build_flags} lib_deps = ${ESPAsyncWebServer.lib_deps} https://github.com/bblanchon/ArduinoJson.git#v7.0.3 ; https://github.com/Jason2866/ESP32_Show_Info.git ${STARBASE_USERMOD_E131.lib_deps} + ${STARBASE_USERMOD_MPU6050.lib_deps} ; ${STARBASE_USERMOD_HA.lib_deps} diff --git a/src/Sys/SysModModel.cpp b/src/Sys/SysModModel.cpp index 03778f12..4d3f1bfc 100644 --- a/src/Sys/SysModModel.cpp +++ b/src/Sys/SysModModel.cpp @@ -287,10 +287,10 @@ bool SysModModel::callVarChangeFun(JsonObject var, unsigned8 rowNr, bool init) { Coord3D *valuePointer = (Coord3D *)pointer; if (valuePointer != nullptr) { *valuePointer = value; - ppf("pointer set16 %s: v:%d (p:%p) (r:%d v:%s p:%d)\n", varID(var), *valuePointer, valuePointer, rowNr, var["value"].as().c_str(), pointer); + // ppf("pointer set coord3D %s: v:%d,%d,%d (p:%p) (r:%d v:%s p:%d)\n", varID(var), (*valuePointer).x, (*valuePointer).y, (*valuePointer).z, valuePointer, rowNr, var["value"].as().c_str(), pointer); } else - ppf("dev pointer set16 %s: v:%d (p:%p) (r:%d v:%s p:%d)\n", varID(var), *valuePointer, valuePointer, rowNr, var["value"].as().c_str(), pointer); + ppf("dev pointer set coord3D %s: v:%d,%d,%d (p:%p) (r:%d v:%s p:%d)\n", varID(var), (*valuePointer).x, (*valuePointer).y, (*valuePointer).z, valuePointer, rowNr, var["value"].as().c_str(), pointer); } else ppf("dev pointer of type %s not supported yet\n", var["type"].as().c_str()); diff --git a/src/Sys/SysModPins.cpp b/src/Sys/SysModPins.cpp index b89bc58b..ed536c01 100644 --- a/src/Sys/SysModPins.cpp +++ b/src/Sys/SysModPins.cpp @@ -10,7 +10,6 @@ */ #include "SysModPins.h" -#include "SysModPrint.h" #include "SysModUI.h" #include "SysModWeb.h" diff --git a/src/Sys/SysModPins.h b/src/Sys/SysModPins.h index 301c9f9a..420ab42d 100644 --- a/src/Sys/SysModPins.h +++ b/src/Sys/SysModPins.h @@ -11,6 +11,9 @@ #pragma once #include "SysModule.h" +#include "SysModPrint.h" + +#include "Wire.h" //for I2S #define pinTypeIO 0 #define pinTypeReadOnly 1 @@ -112,6 +115,15 @@ class SysModPins:public SysModule { return pinType; } + + bool initI2S () { + //tbd: set pins in ui!! + allocatePin(21, "Pins", "I2S SDA"); + allocatePin(22, "Pins", "I2S SCL"); + bool success = Wire.begin(21,22); + ppf("initI2S Wire begin ...\n", success?"success":"failure"); + return success; + } }; extern SysModPins *pins; \ No newline at end of file diff --git a/src/User/UserModMPU6050.h b/src/User/UserModMPU6050.h new file mode 100644 index 00000000..0403ab1f --- /dev/null +++ b/src/User/UserModMPU6050.h @@ -0,0 +1,138 @@ +/* + @title StarBase + @file UserModMPU6050.h + @date 20240411 + @repo https://github.com/ewowi/StarBase, submit changes to this file as PRs to ewowi/StarBase + @Authors https://github.com/ewowi/StarBase/commits/main + @Copyright © 2024 Github StarBase Commit Authors + @license GNU GENERAL PUBLIC LICENSE Version 3, 29 June 2007 + @license For non GPL-v3 usage, commercial licenses must be purchased. Contact moonmodules@icloud.com +*/ + +#include "../sys/SysModUI.h" //why needed here and not in other sysmods? +#include "../sys/SysModPins.h" + +#include + +//see https://github.com/ElectronicCats/mpu6050/blob/0281cd4532b36922f4d68a4cae70eca7aebe9988/examples/MPU6050_DMP6/MPU6050_DMP6.ino + +class UserModMPU6050: public SysModule { + +public: + + Coord3D gyro; // in degrees (not radians) + Coord3D accell; + + UserModMPU6050() :SysModule("Motion Tracking") { + // isEnabled = false; + }; + + void setup() { + SysModule::setup(); + parentVar = ui->initUserMod(parentVar, name, 6305); + + ui->initCoord3D(parentVar, "gyro", &gyro, 0, UINT16_MAX, true, [](JsonObject var, unsigned8 rowNr, unsigned8 funType) { switch (funType) { //varFun + case f_UIFun: + ui->setComment(var, "in degrees"); + return true; + default: return false; + }}); + + ui->initCoord3D(parentVar, "accell", &accell, 0, UINT16_MAX, true, [](JsonObject var, unsigned8 rowNr, unsigned8 funType) { switch (funType) { //varFun + case f_UIFun: + ui->setComment(var, "in m/s²"); + return true; + default: return false; + }}); + + if (pins->initI2S()) { + mpu.initialize(); + + // verify connection + ppf("Testing device connections %s\n", mpu.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed"); + + ppf("Initializing DMP...\n"); + uint8_t devStatus = mpu.dmpInitialize(); + + if (devStatus == 0) { + // // Calibration Time: generate offsets and calibrate our MPU6050 + mpu.CalibrateAccel(6); + mpu.CalibrateGyro(6); + // mpu.PrintActiveOffsets(); + + mpu.setDMPEnabled(true); //mandatory + + // mpuIntStatus = mpu.getIntStatus(); + + dmpReady = true; + } + else { + // ERROR! + // 1 = initial memory load failed + // 2 = DMP configuration updates failed + // (if it's going to break, usually the code will be 1) + ppf("DMP Initialization failed (code %d)\n", devStatus); + } + } + } + + void loop() { + // mpu.getMotion6(&accell.x, &accell.y, &accell.z, &gyro.x, &gyro.y, &gyro.z); + // // display tab-separated accel/gyro x/y/z values + // ppf("mpu6050 %d,%d,%d %d,%d,%d\n", accell.x, accell.y, accell.z, gyro.x, gyro.y, gyro.z); + + // if programming failed, don't try to do anything + if (!dmpReady) return; + // read a packet from FIFO + if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer)) { // Get the Latest packet + mpu.dmpGetQuaternion(&q, fifoBuffer); + mpu.dmpGetGravity(&gravity, &q); + mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); + gyro.y = ypr[0] * 180/M_PI; //pan = yaw ! + gyro.x = ypr[1] * 180/M_PI; //tilt = pitch ! + gyro.z = ypr[2] * 180/M_PI; //roll = roll + + // display real acceleration, adjusted to remove gravity + + //needed to repeat the following 3 lines (yes if you look at the output: otherwise not 0) + mpu.dmpGetQuaternion(&q, fifoBuffer); + mpu.dmpGetAccel(&aa, fifoBuffer); + mpu.dmpGetGravity(&gravity, &q); + + mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity); + // mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q); //worked in 0.6.0, not in 1.3.0 anymore + + accell.x = aaReal.x; + accell.y = aaReal.y; + accell.z = aaReal.z; + } + } + + void loop1s() { + //for debugging + // ppf("mpu6050 ptr:%d,%d,%d ar:%d,%d,%d\n", gyro.x, gyro.y, gyro.z, accell.x, accell.y, accell.z); + + mdl->setValue("gyro", gyro); + mdl->setValue("accell", accell); + } + + private: + MPU6050 mpu; + + // MPU control/status vars + bool dmpReady = false; // set true if DMP init was successful + uint8_t devStatus; // return status after each device operation (0 = success, !0 = error) + uint8_t fifoBuffer[64]; // FIFO storage buffer + + // orientation/motion vars + 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 + // VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements + VectorFloat gravity; // [x, y, z] gravity vector + // float euler[3]; // [psi, theta, phi] Euler angle container + float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector + +}; + +extern UserModMPU6050 *mpu6050; \ No newline at end of file diff --git a/tools/misc.txt b/tools/misc.txt index eb848c66..1105bf09 100644 --- a/tools/misc.txt +++ b/tools/misc.txt @@ -11,6 +11,7 @@ cp ./src/Sys/* ../../ewowi/StarBase/src/Sys cp ./src/User/* ../../ewowi/StarBase/src/User cp ./tools/* ../../ewowi/StarBase/tools cp ./data/index* ../../ewowi/StarBase/data +cp ./platformio.ini ../../ewowi/StarBase search range-based for loop