From 65e8c544d3eb288922fbdf3f46b4ca4f859207fc Mon Sep 17 00:00:00 2001
From: kounocom <71262281+kounocom@users.noreply.github.com>
Date: Fri, 17 Jan 2025 15:09:12 +0200
Subject: [PATCH 01/10] Make I2Cscan non-blocking

---
 lib/i2cscan/i2cscan.cpp       | 218 +++++++++++++++-------------------
 lib/i2cscan/i2cscan.h         |   1 +
 src/main.cpp                  |   1 +
 src/sensors/SensorManager.cpp |   2 +-
 4 files changed, 98 insertions(+), 124 deletions(-)

diff --git a/lib/i2cscan/i2cscan.cpp b/lib/i2cscan/i2cscan.cpp
index 4dde0fca5..1588e8e21 100644
--- a/lib/i2cscan/i2cscan.cpp
+++ b/lib/i2cscan/i2cscan.cpp
@@ -5,16 +5,14 @@
 uint8_t portArray[] = {16, 5, 4, 2, 14, 12, 13};
 uint8_t portExclude[] = {LED_PIN};
 String portMap[] = {"D0", "D1", "D2", "D4", "D5", "D6", "D7"};
-// ESP32C3 has not as many ports as the ESP32
 #elif defined(ESP32C3)
 uint8_t portArray[] = {2, 3, 4, 5, 6, 7, 8, 9, 10};
 uint8_t portExclude[] = {18, 19, 20, 21, LED_PIN};
 String portMap[] = {"2", "3", "4", "5", "6", "7", "8", "9", "10"};
-// this is for the ESP32C6 has a lot of pins (10/11 only availiable on the WROOM modules but not on the "mini")
 #elif defined(ESP32C6)
 uint8_t portArray[] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 14, 15, 18, 19, 20, 21, 22, 23};
 String portMap[] = {"0", "1", "2", "3", "4", "5", "6", "7", "8", "9", "10", "11", "14", "15", "18", "19", "20", "21", "22", "23"};
-uint8_t portExclude[] = {12, 13, 16, 17, LED_PIN}; // exclude USB D+,D- and serial TX/RX
+uint8_t portExclude[] = {12, 13, 16, 17, LED_PIN};
 #elif defined(ESP32)
 uint8_t portArray[] = {4, 13, 14, 15, 16, 17, 18, 19, 21, 22, 23, 25, 26, 27, 32, 33};
 String portMap[] = {"4", "13", "14", "15", "16", "17", "18", "19", "21", "22", "23", "25", "26", "27", "32", "33"};
@@ -23,47 +21,79 @@ uint8_t portExclude[] = {LED_PIN};
 
 namespace I2CSCAN
 {
+    enum ScanState {
+        IDLE,
+        SCANNING,
+        DONE
+    };
+
+    ScanState scanState = IDLE;
+    uint8_t currentI = 0;
+    uint8_t currentJ = 0;
+    uint8_t currentAddress = 1;
+    bool found = false;
 
-    uint8_t pickDevice(uint8_t addr1, uint8_t addr2, bool scanIfNotFound) {
-        if(I2CSCAN::hasDevOnBus(addr1)) {
-            return addr1;
-        }
-        if(I2CSCAN::hasDevOnBus(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");
+    void scani2cports()
+    {
+        if (scanState == IDLE) {
+            found = false;
+            currentI = 0;
+            currentJ = 0;
+            currentAddress = 1;
+            scanState = SCANNING;
         }
-        return 0;
     }
 
-    void scani2cports()
+    void continueScan()
     {
-        bool found = false;
-        for (uint8_t i = 0; i < sizeof(portArray); i++)
+        if (scanState != SCANNING) {
+            return;
+        }
+
+        if (currentAddress == 1) {
+#if ESP32
+            Wire.end();
+#endif
+            Wire.begin((int)portArray[currentI], (int)portArray[currentJ]);
+        }
+
+        Wire.beginTransmission(currentAddress);
+        byte error = Wire.endTransmission();
+
+        if (error == 0)
         {
-            for (uint8_t j = 0; j < sizeof(portArray); j++)
-            {
-                if ((i != j) && !inArray(portArray[i], portExclude, sizeof(portExclude)) && !inArray(portArray[j], portExclude, sizeof(portExclude)))
-                {
-                    if(checkI2C(i, j))
-                        found = true;
-                }
-            }
+            Serial.printf("[DBG] I2C (@ %s(%d) : %s(%d)): I2C device found at address 0x%02x  !\n", 
+                            portMap[currentI].c_str(), portArray[currentI], portMap[currentJ].c_str(), portArray[currentJ], currentAddress);
+            found = true;
         }
-        if(!found) {
-            Serial.println("[ERR] I2C: No I2C devices found");
+        else if (error == 4)
+        {
+            Serial.printf("[ERR] I2C (@ %s(%d) : %s(%d)): Unknown error at address 0x%02x\n",
+                            portMap[currentI].c_str(), portArray[currentI], portMap[currentJ].c_str(), portArray[currentJ], currentAddress);
         }
 
+        currentAddress++;
+        if (currentAddress >= 127) {
+            currentAddress = 1;
+            currentJ++;
+            if (currentJ >= sizeof(portArray)) {
+                currentJ = 0;
+                currentI++;
+            }
+        }
+
+        if (currentI >= sizeof(portArray)) {
+            if (!found) {
+                Serial.println("[ERR] I2C: No I2C devices found");
+            }
+
 #if ESP32
-        Wire.end();
+            Wire.end();
 #endif
 
-        // Reset the I2C interface back to it's original values
-        Wire.begin(static_cast<int>(PIN_IMU_SDA), static_cast<int>(PIN_IMU_SCL));
+            Wire.begin(static_cast<int>(PIN_IMU_SDA), static_cast<int>(PIN_IMU_SCL));
+            scanState = DONE;
+        }
     }
 
     bool inArray(uint8_t value, uint8_t* array, size_t arraySize)
@@ -78,43 +108,6 @@ namespace I2CSCAN
 
         return false;
     }
-    
-    bool checkI2C(uint8_t i, uint8_t j)
-    {
-        bool found = false;
-
-#if ESP32
-        Wire.end();
-#endif
-
-        Wire.begin((int)portArray[i], (int)portArray[j]);
-
-        byte error, address;
-        int nDevices;
-        nDevices = 0;
-        for (address = 1; address < 127; address++)
-        {
-            // The i2c_scanner uses the return value of
-            // the Write.endTransmisstion to see if
-            // a device did acknowledge to the address.
-            Wire.beginTransmission(address);
-            error = Wire.endTransmission();
-
-            if (error == 0)
-            {
-                Serial.printf("[DBG] I2C (@ %s(%d) : %s(%d)): I2C device found at address 0x%02x  !\n", 
-                                portMap[i].c_str(), portArray[i], portMap[j].c_str(), portArray[j], address);
-                nDevices++;
-                found = true;
-            }
-            else if (error == 4)
-            {
-                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 hasDevOnBus(uint8_t addr) {
         byte error;
@@ -133,75 +126,54 @@ namespace I2CSCAN
         return false;
     }
 
-    /**
-     * This routine turns off the I2C bus and clears it
-     * on return SCA and SCL pins are tri-state inputs.
-     * You need to call Wire.begin() after this to re-enable I2C
-     * This routine does NOT use the Wire library at all.
-     *
-     * returns 0 if bus cleared
-     *         1 if SCL held low.
-     *         2 if SDA held low by slave clock stretch for > 2sec
-     *         3 if SDA held low after 20 clocks.
-     * From: http://www.forward.com.au/pfod/ArduinoProgramming/I2C_ClearBus/index.html
-     * (c)2014 Forward Computing and Control Pty. Ltd.
-     * NSW Australia, www.forward.com.au
-     * This code may be freely used for both private and commerical use
-     */
     int clearBus(uint8_t SDA, uint8_t SCL) {
         #if defined(TWCR) && defined(TWEN)
-        TWCR &= ~(_BV(TWEN)); //Disable the Atmel 2-Wire interface so we can control the SDA and SCL pins directly
+        TWCR &= ~(_BV(TWEN));
         #endif
 
-        pinMode(SDA, INPUT_PULLUP); // Make SDA (data) and SCL (clock) pins Inputs with pullup.
+        pinMode(SDA, INPUT_PULLUP);
         pinMode(SCL, INPUT_PULLUP);
 
-        boolean SCL_LOW = (digitalRead(SCL) == LOW); // Check is SCL is Low.
-        if (SCL_LOW) { //If it is held low Arduno cannot become the I2C master. 
-            return 1; //I2C bus error. Could not clear SCL clock line held low
+        boolean SCL_LOW = (digitalRead(SCL) == LOW);
+        if (SCL_LOW) {
+            return 1;
         }
 
-        boolean SDA_LOW = (digitalRead(SDA) == LOW);  // vi. Check SDA input.
-        int clockCount = 20; // > 2x9 clock
+        boolean SDA_LOW = (digitalRead(SDA) == LOW);
+        int clockCount = 20;
 
-        while (SDA_LOW && (clockCount > 0)) { //  vii. If SDA is Low,
+        while (SDA_LOW && (clockCount > 0)) {
             clockCount--;
-        // Note: I2C bus is open collector so do NOT drive SCL or SDA high.
-            pinMode(SCL, INPUT); // release SCL pullup so that when made output it will be LOW
-            pinMode(SCL, OUTPUT); // then clock SCL Low
-            delayMicroseconds(10); //  for >5uS
-            pinMode(SCL, INPUT); // release SCL LOW
-            pinMode(SCL, INPUT_PULLUP); // turn on pullup resistors again
-            // do not force high as slave may be holding it low for clock stretching.
-            delayMicroseconds(10); //  for >5uS
-            // The >5uS is so that even the slowest I2C devices are handled.
-            SCL_LOW = (digitalRead(SCL) == LOW); // Check if SCL is Low.
-            int counter = 20;
-            while (SCL_LOW && (counter > 0)) {  //  loop waiting for SCL to become High only wait 2sec.
-            counter--;
-            delay(100);
+            pinMode(SCL, INPUT);
+            pinMode(SCL, OUTPUT);
+            delayMicroseconds(10);
+            pinMode(SCL, INPUT);
+            pinMode(SCL, INPUT_PULLUP);
+            delayMicroseconds(10);
             SCL_LOW = (digitalRead(SCL) == LOW);
+            int counter = 20;
+            while (SCL_LOW && (counter > 0)) {
+                counter--;
+                delay(100);
+                SCL_LOW = (digitalRead(SCL) == LOW);
             }
-            if (SCL_LOW) { // still low after 2 sec error
-            return 2; // I2C bus error. Could not clear. SCL clock line held low by slave clock stretch for >2sec
+            if (SCL_LOW) {
+                return 2;
             }
-            SDA_LOW = (digitalRead(SDA) == LOW); //   and check SDA input again and loop
+            SDA_LOW = (digitalRead(SDA) == LOW);
         }
-        if (SDA_LOW) { // still low
-            return 3; // I2C bus error. Could not clear. SDA data line held low
+        if (SDA_LOW) {
+            return 3;
         }
 
-        // else pull SDA line low for Start or Repeated Start
-        pinMode(SDA, INPUT); // remove pullup.
-        pinMode(SDA, OUTPUT);  // and then make it LOW i.e. send an I2C Start or Repeated start control.
-        // When there is only one I2C master a Start or Repeat Start has the same function as a Stop and clears the bus.
-        /// A Repeat Start is a Start occurring after a Start with no intervening Stop.
-        delayMicroseconds(10); // wait >5uS
-        pinMode(SDA, INPUT); // remove output low
-        pinMode(SDA, INPUT_PULLUP); // and make SDA high i.e. send I2C STOP control.
-        delayMicroseconds(10); // x. wait >5uS
-        pinMode(SDA, INPUT); // and reset pins as tri-state inputs which is the default state on reset
+        pinMode(SDA, INPUT);
+        pinMode(SDA, OUTPUT);
+        delayMicroseconds(10);
+        pinMode(SDA, INPUT);
+        pinMode(SDA, INPUT_PULLUP);
+        delayMicroseconds(10);
+        pinMode(SDA, INPUT);
         pinMode(SCL, INPUT);
-        return 0; // all ok
+        return 0;
     }
-}
+}
\ No newline at end of file
diff --git a/lib/i2cscan/i2cscan.h b/lib/i2cscan/i2cscan.h
index a0234db34..c09398263 100644
--- a/lib/i2cscan/i2cscan.h
+++ b/lib/i2cscan/i2cscan.h
@@ -6,6 +6,7 @@
 
 namespace I2CSCAN {
     void scani2cports();
+    void continueScan();
     bool checkI2C(uint8_t i, uint8_t j);
     bool hasDevOnBus(uint8_t addr);
     uint8_t pickDevice(uint8_t addr1, uint8_t addr2, bool scanIfNotFound);
diff --git a/src/main.cpp b/src/main.cpp
index 36bc0f045..f59a74821 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -118,6 +118,7 @@ void loop() {
 	sensorManager.update();
 	battery.Loop();
 	ledManager.update();
+	I2CSCAN::continueScan();
 #ifdef TARGET_LOOPTIME_MICROS
 	long elapsed = (micros() - loopTime);
 	if (elapsed < TARGET_LOOPTIME_MICROS) {
diff --git a/src/sensors/SensorManager.cpp b/src/sensors/SensorManager.cpp
index 9cc2a621e..d29e85c20 100644
--- a/src/sensors/SensorManager.cpp
+++ b/src/sensors/SensorManager.cpp
@@ -122,7 +122,7 @@ void SensorManager::setup() {
 	if (activeSensorCount == 0) {
 		m_Logger.error(
 			"Can't find I2C device on provided addresses, scanning for all I2C devices "
-			"and returning"
+			"in the background"
 		);
 		I2CSCAN::scani2cports();
 	}

From 9e325be04d59beaf19796f67c8de3b1857b01ff3 Mon Sep 17 00:00:00 2001
From: kounocom <71262281+kounocom@users.noreply.github.com>
Date: Fri, 17 Jan 2025 15:42:34 +0200
Subject: [PATCH 02/10] Address suggestions

---
 lib/i2cscan/i2cscan.cpp | 45 +++++++++++++++++++----------------------
 lib/i2cscan/i2cscan.h   |  2 +-
 src/main.cpp            |  2 +-
 3 files changed, 23 insertions(+), 26 deletions(-)

diff --git a/lib/i2cscan/i2cscan.cpp b/lib/i2cscan/i2cscan.cpp
index 1588e8e21..1d7865e2e 100644
--- a/lib/i2cscan/i2cscan.cpp
+++ b/lib/i2cscan/i2cscan.cpp
@@ -3,50 +3,47 @@
 
 #ifdef ESP8266
 uint8_t portArray[] = {16, 5, 4, 2, 14, 12, 13};
-uint8_t portExclude[] = {LED_PIN};
 String portMap[] = {"D0", "D1", "D2", "D4", "D5", "D6", "D7"};
 #elif defined(ESP32C3)
 uint8_t portArray[] = {2, 3, 4, 5, 6, 7, 8, 9, 10};
-uint8_t portExclude[] = {18, 19, 20, 21, LED_PIN};
 String portMap[] = {"2", "3", "4", "5", "6", "7", "8", "9", "10"};
 #elif defined(ESP32C6)
 uint8_t portArray[] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 14, 15, 18, 19, 20, 21, 22, 23};
 String portMap[] = {"0", "1", "2", "3", "4", "5", "6", "7", "8", "9", "10", "11", "14", "15", "18", "19", "20", "21", "22", "23"};
-uint8_t portExclude[] = {12, 13, 16, 17, LED_PIN};
 #elif defined(ESP32)
 uint8_t portArray[] = {4, 13, 14, 15, 16, 17, 18, 19, 21, 22, 23, 25, 26, 27, 32, 33};
 String portMap[] = {"4", "13", "14", "15", "16", "17", "18", "19", "21", "22", "23", "25", "26", "27", "32", "33"};
-uint8_t portExclude[] = {LED_PIN};
 #endif
 
 namespace I2CSCAN
 {
-    enum ScanState {
+    enum class ScanState {
         IDLE,
         SCANNING,
         DONE
     };
 
-    ScanState scanState = IDLE;
-    uint8_t currentI = 0;
-    uint8_t currentJ = 0;
+    ScanState scanState = ScanState::IDLE;
+    uint8_t currentSDA = 0;
+    uint8_t currentSCL = 0;
     uint8_t currentAddress = 1;
     bool found = false;
 
     void scani2cports()
     {
-        if (scanState == IDLE) {
+        if (scanState != ScanState::IDLE) {
+            return;
+        }
             found = false;
-            currentI = 0;
-            currentJ = 0;
+            currentSDA = 0;
+            currentSCL = 0;
             currentAddress = 1;
-            scanState = SCANNING;
-        }
+            scanState = ScanState::SCANNING;
     }
 
-    void continueScan()
+    void update()
     {
-        if (scanState != SCANNING) {
+        if (scanState != ScanState::SCANNING) {
             return;
         }
 
@@ -54,7 +51,7 @@ namespace I2CSCAN
 #if ESP32
             Wire.end();
 #endif
-            Wire.begin((int)portArray[currentI], (int)portArray[currentJ]);
+            Wire.begin((int)portArray[currentSDA], (int)portArray[currentSCL]);
         }
 
         Wire.beginTransmission(currentAddress);
@@ -63,26 +60,26 @@ namespace I2CSCAN
         if (error == 0)
         {
             Serial.printf("[DBG] I2C (@ %s(%d) : %s(%d)): I2C device found at address 0x%02x  !\n", 
-                            portMap[currentI].c_str(), portArray[currentI], portMap[currentJ].c_str(), portArray[currentJ], currentAddress);
+                            portMap[currentSDA].c_str(), portArray[currentSDA], portMap[currentSCL].c_str(), portArray[currentSCL], currentAddress);
             found = true;
         }
         else if (error == 4)
         {
             Serial.printf("[ERR] I2C (@ %s(%d) : %s(%d)): Unknown error at address 0x%02x\n",
-                            portMap[currentI].c_str(), portArray[currentI], portMap[currentJ].c_str(), portArray[currentJ], currentAddress);
+                            portMap[currentSDA].c_str(), portArray[currentSDA], portMap[currentSCL].c_str(), portArray[currentSCL], currentAddress);
         }
 
         currentAddress++;
         if (currentAddress >= 127) {
             currentAddress = 1;
-            currentJ++;
-            if (currentJ >= sizeof(portArray)) {
-                currentJ = 0;
-                currentI++;
+            currentSCL++;
+            if (currentSCL >= sizeof(portArray)) {
+                currentSCL = 0;
+                currentSDA++;
             }
         }
 
-        if (currentI >= sizeof(portArray)) {
+        if (currentSDA >= sizeof(portArray)) {
             if (!found) {
                 Serial.println("[ERR] I2C: No I2C devices found");
             }
@@ -92,7 +89,7 @@ namespace I2CSCAN
 #endif
 
             Wire.begin(static_cast<int>(PIN_IMU_SDA), static_cast<int>(PIN_IMU_SCL));
-            scanState = DONE;
+            scanState = ScanState::DONE;
         }
     }
 
diff --git a/lib/i2cscan/i2cscan.h b/lib/i2cscan/i2cscan.h
index c09398263..3fa2eaf2e 100644
--- a/lib/i2cscan/i2cscan.h
+++ b/lib/i2cscan/i2cscan.h
@@ -6,7 +6,7 @@
 
 namespace I2CSCAN {
     void scani2cports();
-    void continueScan();
+    void update();
     bool checkI2C(uint8_t i, uint8_t j);
     bool hasDevOnBus(uint8_t addr);
     uint8_t pickDevice(uint8_t addr1, uint8_t addr2, bool scanIfNotFound);
diff --git a/src/main.cpp b/src/main.cpp
index f59a74821..0d37a532a 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -118,7 +118,7 @@ void loop() {
 	sensorManager.update();
 	battery.Loop();
 	ledManager.update();
-	I2CSCAN::continueScan();
+	I2CSCAN::update();
 #ifdef TARGET_LOOPTIME_MICROS
 	long elapsed = (micros() - loopTime);
 	if (elapsed < TARGET_LOOPTIME_MICROS) {

From 20520c05007e1f5639accd67c8ffcdd072a2a922 Mon Sep 17 00:00:00 2001
From: kounocom <71262281+kounocom@users.noreply.github.com>
Date: Sun, 19 Jan 2025 07:26:09 +0200
Subject: [PATCH 03/10] Use portExclude again

---
 lib/i2cscan/i2cscan.cpp | 66 ++++++++++++++++++++++++++++-------------
 1 file changed, 46 insertions(+), 20 deletions(-)

diff --git a/lib/i2cscan/i2cscan.cpp b/lib/i2cscan/i2cscan.cpp
index 1d7865e2e..7b54bd05e 100644
--- a/lib/i2cscan/i2cscan.cpp
+++ b/lib/i2cscan/i2cscan.cpp
@@ -3,16 +3,20 @@
 
 #ifdef ESP8266
 uint8_t portArray[] = {16, 5, 4, 2, 14, 12, 13};
+uint8_t portExclude[] = {LED_PIN};
 String portMap[] = {"D0", "D1", "D2", "D4", "D5", "D6", "D7"};
 #elif defined(ESP32C3)
 uint8_t portArray[] = {2, 3, 4, 5, 6, 7, 8, 9, 10};
+uint8_t portExclude[] = {18, 19, 20, 21, LED_PIN};
 String portMap[] = {"2", "3", "4", "5", "6", "7", "8", "9", "10"};
 #elif defined(ESP32C6)
 uint8_t portArray[] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 14, 15, 18, 19, 20, 21, 22, 23};
 String portMap[] = {"0", "1", "2", "3", "4", "5", "6", "7", "8", "9", "10", "11", "14", "15", "18", "19", "20", "21", "22", "23"};
+uint8_t portExclude[] = {12, 13, 16, 17, LED_PIN};
 #elif defined(ESP32)
 uint8_t portArray[] = {4, 13, 14, 15, 16, 17, 18, 19, 21, 22, 23, 25, 26, 27, 32, 33};
 String portMap[] = {"4", "13", "14", "15", "16", "17", "18", "19", "21", "22", "23", "25", "26", "27", "32", "33"};
+uint8_t portExclude[] = {LED_PIN};
 #endif
 
 namespace I2CSCAN
@@ -34,11 +38,11 @@ namespace I2CSCAN
         if (scanState != ScanState::IDLE) {
             return;
         }
-            found = false;
-            currentSDA = 0;
-            currentSCL = 0;
-            currentAddress = 1;
-            scanState = ScanState::SCANNING;
+        found = false;
+        currentSDA = 0;
+        currentSCL = 0;
+        currentAddress = 1;
+        scanState = ScanState::SCANNING;
     }
 
     void update()
@@ -51,6 +55,24 @@ namespace I2CSCAN
 #if ESP32
             Wire.end();
 #endif
+            while (inArray(portArray[currentSDA], portExclude, sizeof(portExclude))) {
+                currentSDA++;
+                if (currentSDA >= sizeof(portArray)) {
+                    scanState = ScanState::DONE;
+                    return;
+                }
+            }
+            while (inArray(portArray[currentSCL], portExclude, sizeof(portExclude))) {
+                currentSCL++;
+                if (currentSCL >= sizeof(portArray)) {
+                    currentSCL = 0;
+                    currentSDA++;
+                    if (currentSDA >= sizeof(portArray)) {
+                        scanState = ScanState::DONE;
+                        return;
+                    }
+                }
+            }
             Wire.begin((int)portArray[currentSDA], (int)portArray[currentSCL]);
         }
 
@@ -73,23 +95,27 @@ namespace I2CSCAN
         if (currentAddress >= 127) {
             currentAddress = 1;
             currentSCL++;
-            if (currentSCL >= sizeof(portArray)) {
-                currentSCL = 0;
-                currentSDA++;
-            }
-        }
-
-        if (currentSDA >= sizeof(portArray)) {
-            if (!found) {
-                Serial.println("[ERR] I2C: No I2C devices found");
-            }
-
+            while (inArray(portArray[currentSCL], portExclude, sizeof(portExclude))) {
+                currentSCL++;
+                if (currentSCL >= sizeof(portArray)) {
+                    currentSCL = 0;
+                    currentSDA++;
+                    while (inArray(portArray[currentSDA], portExclude, sizeof(portExclude))) {
+                        currentSDA++;
+                        if (currentSDA >= sizeof(portArray)) {
+                            if (!found) {
+                                Serial.println("[ERR] I2C: No I2C devices found");
+                            }
 #if ESP32
-            Wire.end();
+                            Wire.end();
 #endif
-
-            Wire.begin(static_cast<int>(PIN_IMU_SDA), static_cast<int>(PIN_IMU_SCL));
-            scanState = ScanState::DONE;
+                            Wire.begin(static_cast<int>(PIN_IMU_SDA), static_cast<int>(PIN_IMU_SCL));
+                            scanState = ScanState::DONE;
+                            return;
+                        }
+                    }
+                }
+            }
         }
     }
 

From ffca264fe3658bdb03e27682265c51fb26302e5c Mon Sep 17 00:00:00 2001
From: kounocom <71262281+kounocom@users.noreply.github.com>
Date: Sun, 19 Jan 2025 07:38:00 +0200
Subject: [PATCH 04/10] Re-add some comments

---
 lib/i2cscan/i2cscan.cpp | 27 ++++++++++++++++++++++++---
 1 file changed, 24 insertions(+), 3 deletions(-)

diff --git a/lib/i2cscan/i2cscan.cpp b/lib/i2cscan/i2cscan.cpp
index 7b54bd05e..1d2470716 100644
--- a/lib/i2cscan/i2cscan.cpp
+++ b/lib/i2cscan/i2cscan.cpp
@@ -55,6 +55,7 @@ namespace I2CSCAN
 #if ESP32
             Wire.end();
 #endif
+            // Skip excluded SDA ports
             while (inArray(portArray[currentSDA], portExclude, sizeof(portExclude))) {
                 currentSDA++;
                 if (currentSDA >= sizeof(portArray)) {
@@ -62,6 +63,7 @@ namespace I2CSCAN
                     return;
                 }
             }
+            // Skip excluded SCL ports
             while (inArray(portArray[currentSCL], portExclude, sizeof(portExclude))) {
                 currentSCL++;
                 if (currentSCL >= sizeof(portArray)) {
@@ -95,11 +97,13 @@ namespace I2CSCAN
         if (currentAddress >= 127) {
             currentAddress = 1;
             currentSCL++;
+            // Skip excluded SCL ports
             while (inArray(portArray[currentSCL], portExclude, sizeof(portExclude))) {
                 currentSCL++;
                 if (currentSCL >= sizeof(portArray)) {
                     currentSCL = 0;
                     currentSDA++;
+                    // Skip excluded SDA ports
                     while (inArray(portArray[currentSDA], portExclude, sizeof(portExclude))) {
                         currentSDA++;
                         if (currentSDA >= sizeof(portArray)) {
@@ -139,6 +143,7 @@ namespace I2CSCAN
         do {
 #endif
             Wire.beginTransmission(addr);
+            // The return value of endTransmission is used to determine if a device is present
             error = Wire.endTransmission();
 #if ESP32C3
         }
@@ -149,9 +154,25 @@ namespace I2CSCAN
         return false;
     }
 
+    /**
+     * This routine turns off the I2C bus and clears it
+     * on return SCA and SCL pins are tri-state inputs.
+     * You need to call Wire.begin() after this to re-enable I2C
+     * This routine does NOT use the Wire library at all.
+     *
+     * returns 0 if bus cleared
+     *         1 if SCL held low.
+     *         2 if SDA held low by slave clock stretch for > 2sec
+     *         3 if SDA held low after 20 clocks.
+     * From: http://www.forward.com.au/pfod/ArduinoProgramming/I2C_ClearBus/index.html
+     * (c)2014 Forward Computing and Control Pty. Ltd.
+     * NSW Australia, www.forward.com.au
+     * This code may be freely used for both private and commerical use
+     */
+
     int clearBus(uint8_t SDA, uint8_t SCL) {
         #if defined(TWCR) && defined(TWEN)
-        TWCR &= ~(_BV(TWEN));
+        TWCR &= ~(_BV(TWEN)); // Disable the Atmel 2-Wire interface so we can control the SDA and SCL pins directly
         #endif
 
         pinMode(SDA, INPUT_PULLUP);
@@ -159,11 +180,11 @@ namespace I2CSCAN
 
         boolean SCL_LOW = (digitalRead(SCL) == LOW);
         if (SCL_LOW) {
-            return 1;
+            return 1; // I2C bus error. Could not clear SCL, clock line held low.
         }
 
         boolean SDA_LOW = (digitalRead(SDA) == LOW);
-        int clockCount = 20;
+        int clockCount = 20; // > 2x9 clock
 
         while (SDA_LOW && (clockCount > 0)) {
             clockCount--;

From b2b04c02c485f5f51903a8a7c09894bd12169afe Mon Sep 17 00:00:00 2001
From: kounocom <71262281+kounocom@users.noreply.github.com>
Date: Sun, 19 Jan 2025 08:18:50 +0200
Subject: [PATCH 05/10] It's 8 AM and I'm allowed to be stupid

---
 lib/i2cscan/i2cscan.cpp | 65 +++++++++++++++++++++--------------------
 1 file changed, 34 insertions(+), 31 deletions(-)

diff --git a/lib/i2cscan/i2cscan.cpp b/lib/i2cscan/i2cscan.cpp
index 1d2470716..269d509aa 100644
--- a/lib/i2cscan/i2cscan.cpp
+++ b/lib/i2cscan/i2cscan.cpp
@@ -32,12 +32,21 @@ namespace I2CSCAN
     uint8_t currentSCL = 0;
     uint8_t currentAddress = 1;
     bool found = false;
+    std::vector<uint8_t> validPorts;
 
     void scani2cports()
     {
         if (scanState != ScanState::IDLE) {
             return;
         }
+
+        // Filter out excluded ports
+        for (size_t i = 0; i < sizeof(portArray); i++) {
+            if (!inArray(portArray[i], portExclude, sizeof(portExclude))) {
+                validPorts.push_back(portArray[i]);
+            }
+        }
+
         found = false;
         currentSDA = 0;
         currentSCL = 0;
@@ -55,27 +64,22 @@ namespace I2CSCAN
 #if ESP32
             Wire.end();
 #endif
-            // Skip excluded SDA ports
-            while (inArray(portArray[currentSDA], portExclude, sizeof(portExclude))) {
-                currentSDA++;
-                if (currentSDA >= sizeof(portArray)) {
-                    scanState = ScanState::DONE;
-                    return;
-                }
-            }
-            // Skip excluded SCL ports
-            while (inArray(portArray[currentSCL], portExclude, sizeof(portExclude))) {
+            while (true) {
+                // Select next port
                 currentSCL++;
-                if (currentSCL >= sizeof(portArray)) {
+                if (currentSCL >= validPorts.size()) {
                     currentSCL = 0;
                     currentSDA++;
-                    if (currentSDA >= sizeof(portArray)) {
+                    if (currentSDA >= validPorts.size()) {
                         scanState = ScanState::DONE;
                         return;
                     }
                 }
+
+                // Check port
+                Wire.begin((int)validPorts[currentSDA], (int)validPorts[currentSCL]);
+                break;
             }
-            Wire.begin((int)portArray[currentSDA], (int)portArray[currentSCL]);
         }
 
         Wire.beginTransmission(currentAddress);
@@ -84,41 +88,40 @@ namespace I2CSCAN
         if (error == 0)
         {
             Serial.printf("[DBG] I2C (@ %s(%d) : %s(%d)): I2C device found at address 0x%02x  !\n", 
-                            portMap[currentSDA].c_str(), portArray[currentSDA], portMap[currentSCL].c_str(), portArray[currentSCL], currentAddress);
+                            portMap[currentSDA].c_str(), validPorts[currentSDA], portMap[currentSCL].c_str(), validPorts[currentSCL], currentAddress);
             found = true;
         }
         else if (error == 4)
         {
             Serial.printf("[ERR] I2C (@ %s(%d) : %s(%d)): Unknown error at address 0x%02x\n",
-                            portMap[currentSDA].c_str(), portArray[currentSDA], portMap[currentSCL].c_str(), portArray[currentSCL], currentAddress);
+                            portMap[currentSDA].c_str(), validPorts[currentSDA], portMap[currentSCL].c_str(), validPorts[currentSCL], currentAddress);
         }
 
         currentAddress++;
         if (currentAddress >= 127) {
             currentAddress = 1;
-            currentSCL++;
-            // Skip excluded SCL ports
-            while (inArray(portArray[currentSCL], portExclude, sizeof(portExclude))) {
+            while (true) {
+                // Select next port
                 currentSCL++;
-                if (currentSCL >= sizeof(portArray)) {
+                if (currentSCL >= validPorts.size()) {
                     currentSCL = 0;
                     currentSDA++;
-                    // Skip excluded SDA ports
-                    while (inArray(portArray[currentSDA], portExclude, sizeof(portExclude))) {
-                        currentSDA++;
-                        if (currentSDA >= sizeof(portArray)) {
-                            if (!found) {
-                                Serial.println("[ERR] I2C: No I2C devices found");
-                            }
+                    if (currentSDA >= validPorts.size()) {
+                        if (!found) {
+                            Serial.println("[ERR] I2C: No I2C devices found");
+                        }
 #if ESP32
-                            Wire.end();
+                        Wire.end();
 #endif
-                            Wire.begin(static_cast<int>(PIN_IMU_SDA), static_cast<int>(PIN_IMU_SCL));
-                            scanState = ScanState::DONE;
-                            return;
-                        }
+                        Wire.begin(static_cast<int>(PIN_IMU_SDA), static_cast<int>(PIN_IMU_SCL));
+                        scanState = ScanState::DONE;
+                        return;
                     }
                 }
+
+                // Check port
+                Wire.begin((int)validPorts[currentSDA], (int)validPorts[currentSCL]);
+                break;
             }
         }
     }

From d00d25f1f4394c727420e3792e8f474195fe63ac Mon Sep 17 00:00:00 2001
From: kounocom <71262281+kounocom@users.noreply.github.com>
Date: Sun, 19 Jan 2025 08:43:32 +0200
Subject: [PATCH 06/10] No more while(true)

---
 lib/i2cscan/i2cscan.cpp | 25 +++++++++++--------------
 1 file changed, 11 insertions(+), 14 deletions(-)

diff --git a/lib/i2cscan/i2cscan.cpp b/lib/i2cscan/i2cscan.cpp
index 269d509aa..0b67f3763 100644
--- a/lib/i2cscan/i2cscan.cpp
+++ b/lib/i2cscan/i2cscan.cpp
@@ -64,22 +64,19 @@ namespace I2CSCAN
 #if ESP32
             Wire.end();
 #endif
-            while (true) {
-                // Select next port
-                currentSCL++;
-                if (currentSCL >= validPorts.size()) {
-                    currentSCL = 0;
-                    currentSDA++;
-                    if (currentSDA >= validPorts.size()) {
-                        scanState = ScanState::DONE;
-                        return;
-                    }
+            // Select next port
+            currentSCL++;
+            if (currentSCL >= validPorts.size()) {
+                currentSCL = 0;
+                currentSDA++;
+                if (currentSDA >= validPorts.size()) {
+                    scanState = ScanState::DONE;
+                    return;
                 }
-
-                // Check port
-                Wire.begin((int)validPorts[currentSDA], (int)validPorts[currentSCL]);
-                break;
             }
+
+            // Check port
+            Wire.begin((int)validPorts[currentSDA], (int)validPorts[currentSCL]);
         }
 
         Wire.beginTransmission(currentAddress);

From 27d3a097038727eb3d3b445532667942f5e3204e Mon Sep 17 00:00:00 2001
From: kounocom <71262281+kounocom@users.noreply.github.com>
Date: Sun, 19 Jan 2025 08:58:42 +0200
Subject: [PATCH 07/10] More cleanup

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

diff --git a/lib/i2cscan/i2cscan.cpp b/lib/i2cscan/i2cscan.cpp
index 0b67f3763..ae6f0326d 100644
--- a/lib/i2cscan/i2cscan.cpp
+++ b/lib/i2cscan/i2cscan.cpp
@@ -65,10 +65,8 @@ namespace I2CSCAN
             Wire.end();
 #endif
             // Select next port
-            currentSCL++;
             if (currentSCL >= validPorts.size()) {
                 currentSCL = 0;
-                currentSDA++;
                 if (currentSDA >= validPorts.size()) {
                     scanState = ScanState::DONE;
                     return;
@@ -97,29 +95,26 @@ namespace I2CSCAN
         currentAddress++;
         if (currentAddress >= 127) {
             currentAddress = 1;
-            while (true) {
-                // Select next port
-                currentSCL++;
-                if (currentSCL >= validPorts.size()) {
-                    currentSCL = 0;
-                    currentSDA++;
-                    if (currentSDA >= validPorts.size()) {
-                        if (!found) {
-                            Serial.println("[ERR] I2C: No I2C devices found");
-                        }
+            // Select next port
+            currentSCL++;
+            if (currentSCL >= validPorts.size()) {
+                currentSCL = 0;
+                currentSDA++;
+                if (currentSDA >= validPorts.size()) {
+                    if (!found) {
+                        Serial.println("[ERR] I2C: No I2C devices found");
+                    }
 #if ESP32
-                        Wire.end();
+                    Wire.end();
 #endif
-                        Wire.begin(static_cast<int>(PIN_IMU_SDA), static_cast<int>(PIN_IMU_SCL));
-                        scanState = ScanState::DONE;
-                        return;
-                    }
+                    Wire.begin(static_cast<int>(PIN_IMU_SDA), static_cast<int>(PIN_IMU_SCL));
+                    scanState = ScanState::DONE;
+                    return;
                 }
-
-                // Check port
-                Wire.begin((int)validPorts[currentSDA], (int)validPorts[currentSCL]);
-                break;
             }
+
+            // Check port
+            Wire.begin((int)validPorts[currentSDA], (int)validPorts[currentSCL]);
         }
     }
 

From 69de1c0836d35fb49647d4d09959b9abdaea0dcb Mon Sep 17 00:00:00 2001
From: kounocom <71262281+kounocom@users.noreply.github.com>
Date: Sun, 19 Jan 2025 09:07:51 +0200
Subject: [PATCH 08/10] ...and more

---
 lib/i2cscan/i2cscan.cpp | 70 ++++++++++++++++++++---------------------
 1 file changed, 35 insertions(+), 35 deletions(-)

diff --git a/lib/i2cscan/i2cscan.cpp b/lib/i2cscan/i2cscan.cpp
index ae6f0326d..dabdb8e25 100644
--- a/lib/i2cscan/i2cscan.cpp
+++ b/lib/i2cscan/i2cscan.cpp
@@ -54,6 +54,32 @@ namespace I2CSCAN
         scanState = ScanState::SCANNING;
     }
 
+    bool selectNextPort() {
+        currentSCL++;
+        if (currentSCL < validPorts.size()) {
+            Wire.begin((int)validPorts[currentSDA], (int)validPorts[currentSCL]);
+            return true;
+        }
+
+        currentSCL = 0;
+        currentSDA++;
+        
+        if (currentSDA >= validPorts.size()) {
+            if (!found) {
+                Serial.println("[ERR] I2C: No I2C devices found");
+            }
+#if ESP32
+            Wire.end();
+#endif
+            Wire.begin(static_cast<int>(PIN_IMU_SDA), static_cast<int>(PIN_IMU_SCL));
+            scanState = ScanState::DONE;
+            return false;
+        }
+
+        Wire.begin((int)validPorts[currentSDA], (int)validPorts[currentSCL]);
+        return true;
+    }
+
     void update()
     {
         if (scanState != ScanState::SCANNING) {
@@ -64,17 +90,9 @@ namespace I2CSCAN
 #if ESP32
             Wire.end();
 #endif
-            // Select next port
-            if (currentSCL >= validPorts.size()) {
-                currentSCL = 0;
-                if (currentSDA >= validPorts.size()) {
-                    scanState = ScanState::DONE;
-                    return;
-                }
+            if (!selectNextPort()) {
+                return;
             }
-
-            // Check port
-            Wire.begin((int)validPorts[currentSDA], (int)validPorts[currentSCL]);
         }
 
         Wire.beginTransmission(currentAddress);
@@ -93,29 +111,12 @@ namespace I2CSCAN
         }
 
         currentAddress++;
-        if (currentAddress >= 127) {
-            currentAddress = 1;
-            // Select next port
-            currentSCL++;
-            if (currentSCL >= validPorts.size()) {
-                currentSCL = 0;
-                currentSDA++;
-                if (currentSDA >= validPorts.size()) {
-                    if (!found) {
-                        Serial.println("[ERR] I2C: No I2C devices found");
-                    }
-#if ESP32
-                    Wire.end();
-#endif
-                    Wire.begin(static_cast<int>(PIN_IMU_SDA), static_cast<int>(PIN_IMU_SCL));
-                    scanState = ScanState::DONE;
-                    return;
-                }
-            }
-
-            // Check port
-            Wire.begin((int)validPorts[currentSDA], (int)validPorts[currentSCL]);
+        if (currentAddress < 127) {
+            return;
         }
+
+        currentAddress = 1;
+        selectNextPort();
     }
 
     bool inArray(uint8_t value, uint8_t* array, size_t arraySize)
@@ -137,9 +138,8 @@ namespace I2CSCAN
         int retries = 2;
         do {
 #endif
-            Wire.beginTransmission(addr);
-            // The return value of endTransmission is used to determine if a device is present
-            error = Wire.endTransmission();
+            Wire.beginTransmission(addr);    
+            error = Wire.endTransmission(); // The return value of endTransmission is used to determine if a device is present
 #if ESP32C3
         }
         while (error != 0 && retries--);

From 5f2cba34066dc03a7cb8997eae6b89a8dcc70e5c Mon Sep 17 00:00:00 2001
From: kounocom <71262281+kounocom@users.noreply.github.com>
Date: Sun, 19 Jan 2025 09:37:34 +0200
Subject: [PATCH 09/10] even more!

---
 lib/i2cscan/i2cscan.cpp | 7 ++-----
 1 file changed, 2 insertions(+), 5 deletions(-)

diff --git a/lib/i2cscan/i2cscan.cpp b/lib/i2cscan/i2cscan.cpp
index dabdb8e25..00ab4d0db 100644
--- a/lib/i2cscan/i2cscan.cpp
+++ b/lib/i2cscan/i2cscan.cpp
@@ -89,10 +89,7 @@ namespace I2CSCAN
         if (currentAddress == 1) {
 #if ESP32
             Wire.end();
-#endif
-            if (!selectNextPort()) {
-                return;
-            }
+#endif   
         }
 
         Wire.beginTransmission(currentAddress);
@@ -111,7 +108,7 @@ namespace I2CSCAN
         }
 
         currentAddress++;
-        if (currentAddress < 127) {
+        if (currentAddress <= 127) {
             return;
         }
 

From c06043ec065b1b29931f29d3bb1017f5a2b0e3dd Mon Sep 17 00:00:00 2001
From: kounocom <71262281+kounocom@users.noreply.github.com>
Date: Sun, 19 Jan 2025 18:42:44 +0200
Subject: [PATCH 10/10] Thanks clang-format

---
 lib/i2cscan/i2cscan.cpp | 10 +++++-----
 1 file changed, 5 insertions(+), 5 deletions(-)

diff --git a/lib/i2cscan/i2cscan.cpp b/lib/i2cscan/i2cscan.cpp
index 00ab4d0db..0573094e4 100644
--- a/lib/i2cscan/i2cscan.cpp
+++ b/lib/i2cscan/i2cscan.cpp
@@ -63,7 +63,7 @@ namespace I2CSCAN
 
         currentSCL = 0;
         currentSDA++;
-        
+
         if (currentSDA >= validPorts.size()) {
             if (!found) {
                 Serial.println("[ERR] I2C: No I2C devices found");
@@ -89,7 +89,7 @@ namespace I2CSCAN
         if (currentAddress == 1) {
 #if ESP32
             Wire.end();
-#endif   
+#endif
         }
 
         Wire.beginTransmission(currentAddress);
@@ -97,7 +97,7 @@ namespace I2CSCAN
 
         if (error == 0)
         {
-            Serial.printf("[DBG] I2C (@ %s(%d) : %s(%d)): I2C device found at address 0x%02x  !\n", 
+            Serial.printf("[DBG] I2C (@ %s(%d) : %s(%d)): I2C device found at address 0x%02x  !\n",
                             portMap[currentSDA].c_str(), validPorts[currentSDA], portMap[currentSCL].c_str(), validPorts[currentSCL], currentAddress);
             found = true;
         }
@@ -120,7 +120,7 @@ namespace I2CSCAN
     {
         for (size_t i = 0; i < arraySize; i++)
         {
-            if (value == array[i]) 
+            if (value == array[i])
             {
                 return true;
             }
@@ -135,7 +135,7 @@ namespace I2CSCAN
         int retries = 2;
         do {
 #endif
-            Wire.beginTransmission(addr);    
+            Wire.beginTransmission(addr);
             error = Wire.endTransmission(); // The return value of endTransmission is used to determine if a device is present
 #if ESP32C3
         }