From 4e454331d469bfcd531e72034a5c2c5e446acde7 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Xose=20P=C3=A9rez?= <xose.perez@gmail.com>
Date: Wed, 20 Dec 2017 17:55:09 +0100
Subject: [PATCH] Improved I2C auto-discover avoiding collissions

---
 code/espurna/config/prototypes.h         |  4 ++-
 code/espurna/config/sensors.h            |  4 +--
 code/espurna/i2c.ino                     | 46 ++++++++++++++++++++++--
 code/espurna/sensors/BMX280Sensor.h      | 16 +++------
 code/espurna/sensors/BaseSensor.h        | 32 +++++++++++++++++
 code/espurna/sensors/EmonADC121Sensor.h  | 11 ++----
 code/espurna/sensors/EmonADS1X15Sensor.h | 11 ++----
 code/espurna/sensors/SI7021Sensor.h      | 14 +++-----
 8 files changed, 95 insertions(+), 43 deletions(-)

diff --git a/code/espurna/config/prototypes.h b/code/espurna/config/prototypes.h
index a3018c43..b6bfb588 100644
--- a/code/espurna/config/prototypes.h
+++ b/code/espurna/config/prototypes.h
@@ -51,7 +51,9 @@ template<typename T> String getSetting(const String& key, unsigned int index, T
 // -----------------------------------------------------------------------------
 // I2C
 // -----------------------------------------------------------------------------
-unsigned char i2cFindFirst(size_t size, unsigned char * addresses);
+unsigned char i2cFindAndLock(size_t size, unsigned char * addresses);
+bool i2cGetLock(unsigned char address);
+bool i2cReleaseLock(unsigned char address);
 
 // -----------------------------------------------------------------------------
 // Debug
diff --git a/code/espurna/config/sensors.h b/code/espurna/config/sensors.h
index fa0e6a35..e25d0302 100644
--- a/code/espurna/config/sensors.h
+++ b/code/espurna/config/sensors.h
@@ -93,7 +93,7 @@
 //--------------------------------------------------------------------------------
 
 #ifndef BMX280_SUPPORT
-#define BMX280_SUPPORT                  0
+#define BMX280_SUPPORT                  1
 #endif
 
 #ifndef BMX280_ADDRESS
@@ -283,7 +283,7 @@
 //--------------------------------------------------------------------------------
 
 #ifndef SI7021_SUPPORT
-#define SI7021_SUPPORT                  0
+#define SI7021_SUPPORT                  1
 #endif
 
 #ifndef SI7021_ADDRESS
diff --git a/code/espurna/i2c.ino b/code/espurna/i2c.ino
index e914b35d..77a83e63 100644
--- a/code/espurna/i2c.ino
+++ b/code/espurna/i2c.ino
@@ -8,6 +8,9 @@ Copyright (C) 2017 by Xose Pérez <xose dot perez at gmail dot com>
 
 #if I2C_SUPPORT
 
+#include <vector>
+std::vector<unsigned char> _i2c_locked;
+
 #if I2C_USE_BRZO
 #include "brzo_i2c.h"
 unsigned long _i2c_scl_frequency = 0;
@@ -123,13 +126,50 @@ bool i2cCheck(unsigned char address) {
     #endif
 }
 
-unsigned char i2cFindFirst(size_t size, unsigned char * addresses) {
-    for (unsigned char i=0; i<size; i++) {
-        if (i2cCheck(addresses[i]) == 0) return addresses[i];
+bool i2cGetLock(unsigned char address) {
+    for (unsigned char i=0; i<_i2c_locked.size(); i++) {
+        if (_i2c_locked[i] == address) return false;
+    }
+    _i2c_locked.push_back(address);
+    DEBUG_MSG_P(PSTR("[I2C] Address 0x%02X locked\n"), address);
+    return true;
+}
+
+bool i2cReleaseLock(unsigned char address) {
+    for (unsigned char i=0; i<_i2c_locked.size(); i++) {
+        if (_i2c_locked[i] == address) {
+            _i2c_locked.erase(_i2c_locked.begin() + i);
+            return true;
+        }
+    }
+    return false;
+}
+
+unsigned char i2cFind(size_t size, unsigned char * addresses, unsigned char &start) {
+    for (unsigned char i=start; i<size; i++) {
+        if (i2cCheck(addresses[i]) == 0) {
+            start = i;
+            return addresses[i];
+        }
     }
     return 0;
 }
 
+unsigned char i2cFind(size_t size, unsigned char * addresses) {
+    unsigned char start = 0;
+    return i2cFind(size, addresses, start);
+}
+
+unsigned char i2cFindAndLock(size_t size, unsigned char * addresses) {
+    unsigned char start = 0;
+    unsigned char address = 0;
+    while (address = i2cFind(size, addresses, start)) {
+        if (i2cGetLock(address)) break;
+        start++;
+    }
+    return address;
+}
+
 void i2cScan() {
     unsigned char nDevices = 0;
     for (unsigned char address = 1; address < 127; address++) {
diff --git a/code/espurna/sensors/BMX280Sensor.h b/code/espurna/sensors/BMX280Sensor.h
index 93677436..65119580 100644
--- a/code/espurna/sensors/BMX280Sensor.h
+++ b/code/espurna/sensors/BMX280Sensor.h
@@ -35,17 +35,12 @@ class BMX280Sensor : public BaseSensor {
 
             if (!_dirty) return;
             _dirty = false;
+            _chip = 0;
 
-            // Discover
-            if (_address == 0) {
-                unsigned char addresses[] = {0x76, 0x77};
-                _address = i2cFindFirst(2, addresses);
-            }
-            if (_address == 0) {
-                _error = SENSOR_ERROR_UNKNOWN_ID;
-                _chip = 0;
-                return;
-            }
+            // I2C auto-discover
+            unsigned char addresses[] = {0x76, 0x77};
+            _address = lock_i2c(_address, sizeof(addresses), addresses);
+            if (_address == 0) return;
 
             // Init
             init();
@@ -207,7 +202,6 @@ class BMX280Sensor : public BaseSensor {
 
         BME280 * bme;
         unsigned char _chip;
-        unsigned char _address = 0;
         unsigned long _measurement_delay;
 
 };
diff --git a/code/espurna/sensors/BaseSensor.h b/code/espurna/sensors/BaseSensor.h
index abb7c514..483ff16b 100644
--- a/code/espurna/sensors/BaseSensor.h
+++ b/code/espurna/sensors/BaseSensor.h
@@ -46,6 +46,7 @@ typedef enum magnitude_t {
 #define SENSOR_ERROR_TIMEOUT        3       // Response from sensor timed out
 #define SENSOR_ERROR_UNKNOWN_ID     4       // Sensor did not report a known ID
 #define SENSOR_ERROR_CRC            5       // Sensor data corrupted
+#define SENSOR_ERROR_I2C            6       // Wrong or locked I2C address
 
 class BaseSensor {
 
@@ -81,6 +82,34 @@ class BaseSensor {
         // Current value for slot # index
         virtual double value(unsigned char index) {}
 
+        // Specific for I2C sensors
+        unsigned char lock_i2c(unsigned char address, size_t size, unsigned char * addresses) {
+
+            // Check if we should release a previously locked address
+            if (_previous_address != address) {
+                i2cReleaseLock(_previous_address);
+            }
+
+            // If we have already an address, check it is not locked
+            if (address && !i2cGetLock(address)) {
+                _error = SENSOR_ERROR_I2C;
+
+            // If we don't have an address...
+            } else {
+
+                // Trigger auto-discover
+                address = i2cFindAndLock(size, addresses);
+
+                // If still nothing exit with error
+                if (address == 0) _error = SENSOR_ERROR_I2C;
+
+            }
+
+            _previous_address = address;
+            return address;
+
+        }
+
         // Return sensor status (true for ready)
         bool status() { return _error == 0; }
 
@@ -119,6 +148,9 @@ class BaseSensor {
         bool _dirty = true;
         unsigned char _count = 0;
 
+        // I2C
+        unsigned char _previous_address = 0;
+        unsigned char _address = 0;
 
 };
 
diff --git a/code/espurna/sensors/EmonADC121Sensor.h b/code/espurna/sensors/EmonADC121Sensor.h
index 05857136..f06656c0 100644
--- a/code/espurna/sensors/EmonADC121Sensor.h
+++ b/code/espurna/sensors/EmonADC121Sensor.h
@@ -58,14 +58,9 @@ class EmonADC121Sensor : public EmonAnalogSensor {
             _dirty = false;
 
             // Discover
-            if (_address == 0) {
-                unsigned char addresses[] = {0x50, 0x51, 0x52, 0x54, 0x55, 0x56, 0x58, 0x59, 0x5A};
-                _address = i2cFindFirst(9, addresses);
-            }
-            if (_address == 0) {
-                _error = SENSOR_ERROR_UNKNOWN_ID;
-                return;
-            }
+            unsigned char addresses[] = {0x50, 0x51, 0x52, 0x54, 0x55, 0x56, 0x58, 0x59, 0x5A};
+            _address = lock_i2c(_address, sizeof(addresses), addresses);
+            if (_address == 0) return;
 
             // Init sensor
             #if I2C_USE_BRZO
diff --git a/code/espurna/sensors/EmonADS1X15Sensor.h b/code/espurna/sensors/EmonADS1X15Sensor.h
index 1ddbd27c..c4056bc9 100644
--- a/code/espurna/sensors/EmonADS1X15Sensor.h
+++ b/code/espurna/sensors/EmonADS1X15Sensor.h
@@ -141,14 +141,9 @@ class EmonADS1X15Sensor : public EmonSensor {
             _dirty = false;
 
             // Discover
-            if (_address == 0) {
-                unsigned char addresses[] = {0x48, 0x49, 0x4A, 0x4B};
-                _address = i2cFindFirst(4, addresses);
-            }
-            if (_address == 0) {
-                _error = SENSOR_ERROR_UNKNOWN_ID;
-                return;
-            }
+            unsigned char addresses[] = {0x48, 0x49, 0x4A, 0x4B};
+            _address = lock_i2c(_address, sizeof(addresses), addresses);
+            if (_address == 0) return;
 
             // Calculate ports
             _ports = 0;
diff --git a/code/espurna/sensors/SI7021Sensor.h b/code/espurna/sensors/SI7021Sensor.h
index 8840c9e1..8c9f3a1c 100644
--- a/code/espurna/sensors/SI7021Sensor.h
+++ b/code/espurna/sensors/SI7021Sensor.h
@@ -46,15 +46,10 @@ class SI7021Sensor : public BaseSensor {
             if (!_dirty) return;
             _dirty = false;
 
-            // Discover
-            if (_address == 0) {
-                unsigned char addresses[] = {0x40};
-                _address = i2cFindFirst(1, addresses);
-            }
-            if (_address == 0) {
-                _error = SENSOR_ERROR_UNKNOWN_ID;
-                return;
-            }
+            // I2C auto-discover
+            unsigned char addresses[] = {0x40};
+            _address = lock_i2c(_address, sizeof(addresses), addresses);
+            if (_address == 0) return;
 
             // Check device
             #if I2C_USE_BRZO
@@ -182,7 +177,6 @@ class SI7021Sensor : public BaseSensor {
             return String("Unknown");
         }
 
-        unsigned char _address;
         unsigned char _chip;
 
 };