From d9f6479b797eb0002d6b5bbae9f2f614c35f935c Mon Sep 17 00:00:00 2001
From: Ewoud <ewoudwijma@gmail.com>
Date: Fri, 21 Jun 2024 13:27:41 +0200
Subject: [PATCH] MPU6050 init and error handling

SysModPins
- show proper Wire.begin debug info

UserModMPU6050:
- add mtReady checkbox
- improve init and error handling
- rename dmpReady to motionTrackingReady and make public var.
---
 platformio.ini            |  2 +-
 src/Sys/SysModPins.h      |  2 +-
 src/User/UserModMPU6050.h | 64 ++++++++++++++++++++++++---------------
 3 files changed, 41 insertions(+), 27 deletions(-)

diff --git a/platformio.ini b/platformio.ini
index 4edc214e..c4c815ae 100644
--- a/platformio.ini
+++ b/platformio.ini
@@ -52,7 +52,7 @@ lib_deps =
 build_flags = 
   -D APP=StarBase
   -D PIOENV=$PIOENV
-  -D VERSION=24060410 ; Date and time (GMT!), update at every commit!!
+  -D VERSION=24062015 ; Date and time (GMT!), update at every commit!!
   -D CONFIG_ASYNC_TCP_USE_WDT=0
   -D LFS_THREADSAFE            ; enables use of semaphores in LittleFS driver
   -D STARBASE_DEVMODE
diff --git a/src/Sys/SysModPins.h b/src/Sys/SysModPins.h
index 420ab42d..c534d8b5 100644
--- a/src/Sys/SysModPins.h
+++ b/src/Sys/SysModPins.h
@@ -121,7 +121,7 @@ class SysModPins:public SysModule {
     allocatePin(21, "Pins", "I2S SDA");
     allocatePin(22, "Pins", "I2S SCL");
     bool success = Wire.begin(21,22);
-    ppf("initI2S Wire begin ...\n", success?"success":"failure");
+    ppf("initI2S Wire begin %s\n", success?"success":"failure");
     return success;
   }
 };
diff --git a/src/User/UserModMPU6050.h b/src/User/UserModMPU6050.h
index fa40a357..08b908f1 100644
--- a/src/User/UserModMPU6050.h
+++ b/src/User/UserModMPU6050.h
@@ -9,6 +9,8 @@
    @license   For non GPL-v3 usage, commercial licenses must be purchased. Contact moonmodules@icloud.com
 */
 
+#pragma once
+
 #include "../Sys/SysModUI.h" //why needed here and not in other sysmods?
 #include "../Sys/SysModPins.h"
 
@@ -20,6 +22,8 @@ class UserModMPU6050: public SysModule {
 
 public:
 
+  bool motionTrackingReady = false;  // set true if DMP init was successful
+
   Coord3D gyro; // in degrees (not radians)
   Coord3D accell;
 
@@ -31,6 +35,13 @@ class UserModMPU6050: public SysModule {
     SysModule::setup();
     parentVar = ui->initUserMod(parentVar, name, 6305);
 
+    ui->initCheckBox(parentVar, "mtReady", &motionTrackingReady, true, [](JsonObject var, unsigned8 rowNr, unsigned8 funType) { switch (funType) { //varFun
+      case f_UIFun:
+        ui->setLabel(var, "tracking ready");
+        return true;
+      default: return false;
+    }}); 
+
     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");
@@ -49,31 +60,35 @@ class UserModMPU6050: public SysModule {
       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);
+      if (mpu.testConnection()) {
+        ppf("MPU6050 connection successful 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();
+
+          motionTrackingReady = 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);
+        }
       }
+      else
+        ppf("Testing device connections MPU6050 connection failed\n");
     }
+
+    mdl->setValue("mtReady", motionTrackingReady);
   }
 
   void loop() {
@@ -82,7 +97,7 @@ class UserModMPU6050: public SysModule {
     // 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;
+    if (!motionTrackingReady) return;
     // read a packet from FIFO
     if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer)) { // Get the Latest packet 
       mpu.dmpGetQuaternion(&q, fifoBuffer);
@@ -120,7 +135,6 @@ class UserModMPU6050: public SysModule {
     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