Camera: don't bail out on first i2c device found

Author: facchinm

libraries/Camera/src/camera.cpp

@@ -408,21 +408,22 @@ int Camera::reset()
     return 0;
 }
 
-int Camera::probeSensor()
+ScanResults<uint8_t> Camera::i2cScan()
 {
     uint8_t addr;
+    ScanResults<uint8_t> res;
 
     for (addr=1; addr<127; addr++) {
         CameraWire.beginTransmission(addr);
         if (CameraWire.endTransmission() == 0) {
-            break;
+            res.push(addr);
         }
     }
     if (_debug) {
         _debug->print("Sensor address: 0x");
         _debug->println(addr, HEX);
     }
-    return addr;
+    return res;
 }
 
 bool Camera::begin(int32_t resolution, int32_t pixformat, int32_t framerate)
@@ -445,10 +446,13 @@ bool Camera::begin(int32_t resolution, int32_t pixformat, int32_t framerate)
     }
 
     if (this->sensor->init() != 0) {
+        if (_debug) {
+            _debug->print("Sensor init failed");
+        }
         return false;
     }
 
-    if (probeSensor() != this->sensor->getID()) {
+    if (i2cScan() != this->sensor->getID()) {
         if (_debug) {
             _debug->print("Detected SensorID: 0x");
             _debug->println(this->sensor->getID(), HEX);

libraries/Camera/src/camera.h

@@ -88,14 +88,35 @@ class ImageSensor {
         }
 };
 
+template <typename T> class ScanResults {
+public:
+    bool operator==(T toBeFound) {
+        for (int i = 0; i < howMany; i++) {
+            if (toBeFound == data[i]) {
+                return true;
+            }
+        }
+        return false;
+    }
+    bool operator!=(T toBeFound) {
+        return !(*this == toBeFound);
+    }
+    void push(T obj) {
+        data[howMany++] = obj;
+    }
+private:
+    T data[20];
+    int howMany = 0;
+};
+
 class Camera {
     private:
         int32_t pixformat;
         int32_t resolution;
         int32_t framerate;
         ImageSensor *sensor;
         int reset();
-        int probeSensor();
+        ScanResults<uint8_t> i2cScan();
         Stream *_debug;
         arduino::MbedI2C *_i2c;
         FrameBuffer *_framebuffer;