From 214ce9b464b67c0afee65297a87844623c329566 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Przemys=C5=82aw=20Romaniak?= <loudpl@gmail.com>
Date: Mon, 24 Jul 2023 00:17:30 +0200
Subject: [PATCH 1/3] fix uninitialized calibration values and temperature
 coefficients causing all sort of issues on ESP32C3

---
 src/configuration/Configuration.cpp | 6 ++----
 src/configuration/Configuration.h   | 2 --
 src/sensors/bmi160sensor.h          | 4 ++--
 src/sensors/icm20948sensor.h        | 2 +-
 src/sensors/mpu6050sensor.h         | 2 +-
 src/sensors/mpu9250sensor.h         | 4 ++--
 6 files changed, 8 insertions(+), 12 deletions(-)

diff --git a/src/configuration/Configuration.cpp b/src/configuration/Configuration.cpp
index a0264d4b5..74431a063 100644
--- a/src/configuration/Configuration.cpp
+++ b/src/configuration/Configuration.cpp
@@ -32,8 +32,6 @@
 
 namespace SlimeVR {
     namespace Configuration {
-        CalibrationConfig Configuration::m_EmptyCalibration = {NONE};
-
         void Configuration::setup() {
             if (m_Loaded) {
                 return;
@@ -140,7 +138,7 @@ namespace SlimeVR {
 
         CalibrationConfig Configuration::getCalibration(size_t sensorID) const {
             if (sensorID >= m_Calibrations.size()) {
-                return m_EmptyCalibration;
+                return {};
             }
 
             return m_Calibrations.at(sensorID);
@@ -150,7 +148,7 @@ namespace SlimeVR {
             size_t currentCalibrations = m_Calibrations.size();
 
             if (sensorID >= currentCalibrations) {
-                m_Calibrations.resize(sensorID + 1, m_EmptyCalibration);
+                m_Calibrations.resize(sensorID + 1);
             }
 
             m_Calibrations[sensorID] = config;
diff --git a/src/configuration/Configuration.h b/src/configuration/Configuration.h
index 7b191bdb5..4a0eb69e3 100644
--- a/src/configuration/Configuration.h
+++ b/src/configuration/Configuration.h
@@ -60,8 +60,6 @@ namespace SlimeVR {
             std::vector<CalibrationConfig> m_Calibrations;
 
             Logging::Logger m_Logger = Logging::Logger("Configuration");
-
-            static CalibrationConfig m_EmptyCalibration;
         };
     }
 }
diff --git a/src/sensors/bmi160sensor.h b/src/sensors/bmi160sensor.h
index 3c8643a86..9ebfa7850 100644
--- a/src/sensors/bmi160sensor.h
+++ b/src/sensors/bmi160sensor.h
@@ -217,13 +217,13 @@ class BMI160Sensor : public Sensor {
         double gscaleY = BMI160_GSCALE;
         double gscaleZ = BMI160_GSCALE;
 
-        double GOxyzStaticTempCompensated[3];
+        double GOxyzStaticTempCompensated[3] = {0.0, 0.0, 0.0};
 
         bool isGyroCalibrated = false;
         bool isAccelCalibrated = false;
         bool isMagCalibrated = false;
 
-        SlimeVR::Configuration::BMI160CalibrationConfig m_Calibration;
+        SlimeVR::Configuration::BMI160CalibrationConfig m_Calibration = {};
 };
 
 #endif
diff --git a/src/sensors/icm20948sensor.h b/src/sensors/icm20948sensor.h
index dff47a4af..f3e5c36bc 100644
--- a/src/sensors/icm20948sensor.h
+++ b/src/sensors/icm20948sensor.h
@@ -55,7 +55,7 @@ class ICM20948Sensor : public Sensor
     icm_20948_DMP_data_t dmpData{};
     icm_20948_DMP_data_t dmpDataTemp{};
 
-    SlimeVR::Configuration::ICM20948CalibrationConfig m_Calibration;
+    SlimeVR::Configuration::ICM20948CalibrationConfig m_Calibration = {};
 
     SlimeVR::Sensors::SensorFusionDMP sfusion;
 
diff --git a/src/sensors/mpu6050sensor.h b/src/sensors/mpu6050sensor.h
index 2ae02543a..d7a7b6d90 100644
--- a/src/sensors/mpu6050sensor.h
+++ b/src/sensors/mpu6050sensor.h
@@ -53,7 +53,7 @@ class MPU6050Sensor : public Sensor
     SlimeVR::Sensors::SensorFusionDMP sfusion;
 
 #ifndef IMU_MPU6050_RUNTIME_CALIBRATION
-    SlimeVR::Configuration::MPU6050CalibrationConfig m_Calibration;
+    SlimeVR::Configuration::MPU6050CalibrationConfig m_Calibration = {};
 #endif
 };
 
diff --git a/src/sensors/mpu9250sensor.h b/src/sensors/mpu9250sensor.h
index a666b340c..612657974 100644
--- a/src/sensors/mpu9250sensor.h
+++ b/src/sensors/mpu9250sensor.h
@@ -75,8 +75,8 @@ class MPU9250Sensor : public Sensor
     float Mxyz[3]{};
     VectorInt16 rawAccel{};
     Quat correction{0, 0, 0, 0};
-    
-    SlimeVR::Configuration::MPU9250CalibrationConfig m_Calibration;
+
+    SlimeVR::Configuration::MPU9250CalibrationConfig m_Calibration = {};
 
     // outputs to respective member variables
     void parseAccelData(int16_t data[3]);

From 8e1f2c70d06b21a7f4500c0b609311fef47fd777 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Przemys=C5=82aw=20Romaniak?= <loudpl@gmail.com>
Date: Tue, 25 Jul 2023 00:24:57 +0200
Subject: [PATCH 2/3] esp32c3: workaround to i2c issue with secondary sensor
 disconnected

---
 lib/i2cscan/i2cscan.cpp | 33 +++++++++++++++++++++------------
 1 file changed, 21 insertions(+), 12 deletions(-)

diff --git a/lib/i2cscan/i2cscan.cpp b/lib/i2cscan/i2cscan.cpp
index 649dbc7c9..18793542b 100644
--- a/lib/i2cscan/i2cscan.cpp
+++ b/lib/i2cscan/i2cscan.cpp
@@ -20,18 +20,19 @@ namespace I2CSCAN
 {
 
     uint8_t pickDevice(uint8_t addr1, uint8_t addr2, bool scanIfNotFound) {
-        if(I2CSCAN::isI2CExist(addr1))
+        if(I2CSCAN::isI2CExist(addr1)) {
             return addr1;
-        if(!I2CSCAN::isI2CExist(addr2)) {
-            if(scanIfNotFound) {
-                Serial.println("[ERR] I2C: Can't find I2C device on provided addresses, scanning for all I2C devices and returning");
-                I2CSCAN::scani2cports();
-            } else {
-                Serial.println("[ERR] I2C: Can't find I2C device on provided addresses");
-            }
-            return 0;
         }
-        return addr2;
+        if(I2CSCAN::isI2CExist(addr2)) {
+            return addr2;
+        }
+        if (scanIfNotFound) {
+            Serial.println("[ERR] I2C: Can't find I2C device on provided addresses, scanning for all I2C devices and returning");
+            I2CSCAN::scani2cports();
+        } else {
+            Serial.println("[ERR] I2C: Can't find I2C device on provided addresses");
+        }
+        return 0;
     }
 
     void scani2cports()
@@ -111,8 +112,16 @@ namespace I2CSCAN
     }
 
     bool isI2CExist(uint8_t addr) {
-        Wire.beginTransmission(addr);
-        byte error = Wire.endTransmission();
+        byte error;
+#if ESP32C3
+        do {
+#endif
+            Wire.beginTransmission(addr);
+            error = Wire.endTransmission();
+#if ESP32C3
+        }
+        while (error == 5);
+#endif
         if(error == 0)
             return true;
         return false;

From c6a90ccfff90fa79ca5634e93f17d64f41e4e956 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Przemys=C5=82aw=20Romaniak?= <loudpl@gmail.com>
Date: Sat, 29 Jul 2023 00:30:51 +0200
Subject: [PATCH 3/3] esp32c3: during scan retry only once on timeout

---
 lib/i2cscan/i2cscan.cpp       | 11 ++++++-----
 lib/i2cscan/i2cscan.h         |  2 +-
 src/batterymonitor.cpp        |  2 +-
 src/sensors/SensorManager.cpp |  2 +-
 4 files changed, 9 insertions(+), 8 deletions(-)

diff --git a/lib/i2cscan/i2cscan.cpp b/lib/i2cscan/i2cscan.cpp
index 18793542b..ae94247fa 100644
--- a/lib/i2cscan/i2cscan.cpp
+++ b/lib/i2cscan/i2cscan.cpp
@@ -20,10 +20,10 @@ namespace I2CSCAN
 {
 
     uint8_t pickDevice(uint8_t addr1, uint8_t addr2, bool scanIfNotFound) {
-        if(I2CSCAN::isI2CExist(addr1)) {
+        if(I2CSCAN::hasDevOnBus(addr1)) {
             return addr1;
         }
-        if(I2CSCAN::isI2CExist(addr2)) {
+        if(I2CSCAN::hasDevOnBus(addr2)) {
             return addr2;
         }
         if (scanIfNotFound) {
@@ -104,23 +104,24 @@ namespace I2CSCAN
             }
             else if (error == 4)
             {
-                Serial.printf("[ERR] I2C (@ %s(%d) : %s(%d)): Unknow error at address 0x%02x\n", 
+                Serial.printf("[ERR] I2C (@ %s(%d) : %s(%d)): Unknown error at address 0x%02x\n",
                                 portMap[i].c_str(), portArray[i], portMap[j].c_str(), portArray[j], address);
             }
         }
         return found;
     }
 
-    bool isI2CExist(uint8_t addr) {
+    bool hasDevOnBus(uint8_t addr) {
         byte error;
 #if ESP32C3
+        int retries = 1;
         do {
 #endif
             Wire.beginTransmission(addr);
             error = Wire.endTransmission();
 #if ESP32C3
         }
-        while (error == 5);
+        while (error == 5 && retries--);
 #endif
         if(error == 0)
             return true;
diff --git a/lib/i2cscan/i2cscan.h b/lib/i2cscan/i2cscan.h
index 0ace23455..a0234db34 100644
--- a/lib/i2cscan/i2cscan.h
+++ b/lib/i2cscan/i2cscan.h
@@ -7,7 +7,7 @@
 namespace I2CSCAN {
     void scani2cports();
     bool checkI2C(uint8_t i, uint8_t j);
-    bool isI2CExist(uint8_t addr);
+    bool hasDevOnBus(uint8_t addr);
     uint8_t pickDevice(uint8_t addr1, uint8_t addr2, bool scanIfNotFound);
     int clearBus(uint8_t SDA, uint8_t SCL);
     boolean inArray(uint8_t value, uint8_t* arr, size_t arrSize);
diff --git a/src/batterymonitor.cpp b/src/batterymonitor.cpp
index b7ff913d5..42ec85c01 100644
--- a/src/batterymonitor.cpp
+++ b/src/batterymonitor.cpp
@@ -32,7 +32,7 @@ void BatteryMonitor::Setup()
 #if BATTERY_MONITOR == BAT_MCP3021 || BATTERY_MONITOR == BAT_INTERNAL_MCP3021
     for (uint8_t i = 0x48; i < 0x4F; i++)
     {
-        if (I2CSCAN::isI2CExist(i))
+        if (I2CSCAN::hasDevOnBus(i))
         {
             address = i;
             break;
diff --git a/src/sensors/SensorManager.cpp b/src/sensors/SensorManager.cpp
index b3b46f0e9..9818291b1 100644
--- a/src/sensors/SensorManager.cpp
+++ b/src/sensors/SensorManager.cpp
@@ -57,7 +57,7 @@ namespace SlimeVR
             I2CSCAN::clearBus(sdaPin, sclPin);
             swapI2C(sclPin, sdaPin);
 
-            if (I2CSCAN::isI2CExist(address)) {
+            if (I2CSCAN::hasDevOnBus(address)) {
                 m_Logger.trace("IMU %d found at address 0x%02X", sensorID, address);
             } else {
                 sensor = new ErroneousSensor(sensorID, imuType);