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;