Integrate I2C scan into IMU connection debugging process

lib/i2cscan/i2cscan.cpp

@@ -0,0 +1,65 @@
+#include "i2cscan.h"
+
+uint8_t portArray[] = {16, 5, 4, 0, 2, 14, 12, 13};
+String portMap[] = {"D0", "D1", "D2", "D3", "D4", "D5", "D6", "D7"};
+
+namespace I2CSCAN
+{
+    void scani2cports()
+    {
+        bool found = false;
+        for (uint8_t i = 0; i < sizeof(portArray); i++)
+        {
+            for (uint8_t j = 0; j < sizeof(portArray); j++)
+            {
+                if (i != j)
+                {
+                    if(checkI2C(i, j))
+                        found = true;
+                }
+            }
+        }
+        if(!found) {
+            Serial.println("[I2C SCAN] No I2C devices found");
+        }
+    }
+
+    bool checkI2C(uint8_t i, uint8_t j)
+    {
+        bool found = false;
+        Wire.begin(portArray[i], 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.print("[I2C SCAN] (@ " + portMap[i] + " : " + portMap[j] + ") ");
+                Serial.print("I2C device found at address 0x");
+                if (address < 16)
+                    Serial.print("0");
+                Serial.print(address, HEX);
+                Serial.println("  !");
+
+                nDevices++;
+                found = true;
+            }
+            else if (error == 4)
+            {
+                Serial.print("[I2C SCAN] (@ " + portMap[i] + " : " + portMap[j] + ") ");
+                Serial.print("Unknow error at address 0x");
+                if (address < 16)
+                    Serial.print("0");
+                Serial.println(address, HEX);
+            }
+        }
+        return found;
+    }
+}

lib/i2cscan/i2cscan.h

@@ -0,0 +1,12 @@
+#ifndef _I2CSCAN_H_
+#define _I2CSCAN_H_ 1
+
+#include <Arduino.h>
+#include <Wire.h>
+
+namespace I2CSCAN {
+    void scani2cports();
+    bool checkI2C(uint8_t i, uint8_t j);
+}
+
+#endif // _I2CSCAN_H_

src/bno080sensor.cpp

@@ -25,20 +25,28 @@
 #include "sensor.h"
 #include "udpclient.h"
 #include "defines.h"
+#include <i2cscan.h>
 
 void BNO080Sensor::motionSetup(DeviceConfig * config)
 {
     delay(500);
-    if(!imu.begin(BNO080_DEFAULT_ADDRESS, Wire)) {
-        Serial.println("Can't connect to BNO08X");
-        for(int i = 0; i < 500; ++i) {
+    if(FULL_DEBUG)
+        imu.enableDebugging(Serial);
+    if(!imu.begin(IMU_I2C_ADDRESS, Wire)) {
+        Serial.print("Can't connect to ");
+        Serial.println(IMU_NAME);
+        Serial.println("Will scan I2C devices...");
+        I2CSCAN::scani2cports();
+        for(int i = 0; i < 200; ++i) {
             delay(50);
             digitalWrite(LOADING_LED, LOW);
             delay(50);
             digitalWrite(LOADING_LED, HIGH);
         }
+        return;
     }
-    Serial.println("Connected to BNO08X");
+    Serial.print("Connected to ");
+    Serial.println(IMU_NAME);
     Wire.setClock(400000);
     if(BNO_HASARVR_STABILIZATION)
         imu.enableARVRStabilizedGameRotationVector(10);

src/defines.h

@@ -29,32 +29,37 @@
 #define IMU_BNO085 4
 #define IMU_BNO055 5
 
-#define IMU IMU_BNO085
+#define IMU IMU_BNO080
 
 #if IMU == IMU_BNO085
+  #define IMU_I2C_ADDRESS 0x4B
   #define IMU_NAME "BNO085"
   #define IMU_HAS_ACCELL true
   #define IMU_HAS_GYRO true
   #define IMU_HAS_MAG true
   #define BNO_HASARVR_STABILIZATION true
 #elif IMU == IMU_BNO080
+  #define IMU_I2C_ADDRESS 0x4B
   #define IMU_NAME "BNO080"
   #define IMU_HAS_ACCELL true
   #define IMU_HAS_GYRO true
   #define IMU_HAS_MAG true
   #define BNO_HASARVR_STABILIZATION false
 #elif IMU == IMU_BNO055
+  #define IMU_I2C_ADDRESS 0x29
   #define IMU_NAME "BNO055"
   #define IMU_HAS_ACCELL true
   #define IMU_HAS_GYRO true
   #define IMU_HAS_MAG true
   #define BNO_HASARVR_STABILIZATION false
 #elif IMU == IMU_MPU9250
+  #define IMU_I2C_ADDRESS 0x69
   #define IMU_NAME "MPU9250"
   #define IMU_HAS_ACCELL true
   #define IMU_HAS_GYRO true
   #define IMU_HAS_MAG true
 #elif IMU == IMU_MPU6500
+  #define IMU_I2C_ADDRESS 0x69
   #define IMU_NAME "MPU6500"
   #define IMU_HAS_ACCELL true
   #define IMU_HAS_GYRO true