Add Motion Tracking usermod

pio.ini
- add MPU6050 usermod

SysModPins:
- add wire.h and init2S

Add UserModMPU6050

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}
 
 

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<String>().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<String>().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<String>().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<String>().c_str(), pointer);
     }
     else
       ppf("dev pointer of type %s not supported yet\n", var["type"].as<String>().c_str());

src/Sys/SysModPins.cpp

@@ -10,7 +10,6 @@
 */
 
 #include "SysModPins.h"
-#include "SysModPrint.h"
 #include "SysModUI.h"
 #include "SysModWeb.h"
 

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;

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 [email protected]
+*/
+
+#include "../sys/SysModUI.h" //why needed here and not in other sysmods?
+#include "../sys/SysModPins.h"
+
+#include <MPU6050_6Axis_MotionApps20.h>
+
+//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;

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