Check different I2C addresses when connecting to IMU

lib/i2cscan/i2cscan.cpp

@@ -62,4 +62,12 @@ namespace I2CSCAN
         }
         return found;
     }
+
+    bool isI2CExist(uint8_t addr) {
+        Wire.beginTransmission(addr);
+        byte error = Wire.endTransmission();
+        if(error == 0)
+            return true;
+        return false;
+    }
 }

lib/i2cscan/i2cscan.h

@@ -7,6 +7,7 @@
 namespace I2CSCAN {
     void scani2cports();
     bool checkI2C(uint8_t i, uint8_t j);
+    bool isI2CExist(uint8_t addr);
 }
 
 #endif // _I2CSCAN_H_

lib/mpu9250/MPU9250.cpp

@@ -43,20 +43,6 @@ MPU9250::MPU9250() {
     devAddr = MPU9250_DEFAULT_ADDRESS;
 }
 
-/** Specific address constructor.
- * @param address I2C address
- * @see MPU9250_DEFAULT_ADDRESS
- * @see MPU9250_ADDRESS_AD0_LOW
- * @see MPU9250_ADDRESS_AD0_HIGH
- */
-MPU9250::MPU9250(uint8_t address) {
-    devAddr = address;
-}
-
-uint8_t MPU9250::getAddr() {
-    return devAddr;
-}
-
 /** Power on and prepare for general usage.
  * This will activate the device and take it out of sleep mode (which must be done
  * after start-up). This function also sets both the accelerometer and the gyroscope
@@ -64,13 +50,18 @@ uint8_t MPU9250::getAddr() {
  * the clock source to use the X Gyro for reference, which is slightly better than
  * the default internal clock source.
  */
-void MPU9250::initialize() {
+void MPU9250::initialize(uint8_t address) {
+    devAddr = address;
     setClockSource(MPU9250_CLOCK_PLL_XGYRO);
     setFullScaleGyroRange(MPU9250_GYRO_FS_250);
     setFullScaleAccelRange(MPU9250_ACCEL_FS_2);
     setSleepEnabled(false); // thanks to Jack Elston for pointing this one out!
 }
 
+uint8_t MPU9250::getAddr() {
+    return devAddr;
+}
+
 /** Verify the I2C connection.
  * Make sure the device is connected and responds as expected.
  * @return True if connection is valid, false otherwise

lib/mpu9250/MPU9250.h

@@ -412,9 +412,8 @@ THE SOFTWARE.
 class MPU9250 {
     public:
         MPU9250();
-        MPU9250(uint8_t address);
 
-        void initialize();
+        void initialize(uint8_t address = MPU9250_DEFAULT_ADDRESS);
         bool testConnection();
 
         uint8_t getAddr();

src/bno080sensor.cpp

@@ -27,22 +27,34 @@
 #include "defines.h"
 #include <i2cscan.h>
 
+void signalAssert() {
+    for(int i = 0; i < 200; ++i) {
+        delay(50);
+        digitalWrite(LOADING_LED, LOW);
+        delay(50);
+        digitalWrite(LOADING_LED, HIGH);
+    }
+}
+
 void BNO080Sensor::motionSetup(DeviceConfig * config)
 {
+    uint8_t addr = 0x4A;
+    if(!I2CSCAN::isI2CExist(addr)) {
+        addr = 0x4B;
+        if(!I2CSCAN::isI2CExist(addr)) {
+            Serial.println("Can't find I2C device on addr 0x4A or 0x4B, scanning for all I2C devices and returning");
+            I2CSCAN::scani2cports();
+            signalAssert();
+            return;
+        }
+    }
     delay(500);
     if(FULL_DEBUG)
         imu.enableDebugging(Serial);
-    if(!imu.begin(IMU_I2C_ADDRESS, Wire)) {
+    if(!imu.begin(addr, 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);
-        }
+        signalAssert();
         return;
     }
     Serial.print("Connected to ");

src/defines.h

@@ -32,34 +32,29 @@
 #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

src/main.cpp

@@ -78,8 +78,8 @@ void setup()
     pinMode(CALIBRATING_LED, OUTPUT);
     digitalWrite(CALIBRATING_LED, HIGH);
     digitalWrite(LOADING_LED, LOW);
-
-    // join I2C bus (I2Cdev library doesn't do this automatically)
+    
+    // join I2C bus
     Wire.flush();
     Wire.begin(D2, D1);
     Wire.setClockStretchLimit(4000);

src/mpu6050sensor.cpp

@@ -24,14 +24,32 @@
 #include "MPU9250.h"
 #include "sensor.h"
 #include "udpclient.h"
+#include <i2cscan.h>
+
+void signalAssert() {
+    for(int i = 0; i < 200; ++i) {
+        delay(50);
+        digitalWrite(LOADING_LED, LOW);
+        delay(50);
+        digitalWrite(LOADING_LED, HIGH);
+    }
+}
 
 void gatherCalibrationData(MPU9250 &imu);
 
 void MPU6050Sensor::motionSetup(DeviceConfig * config) {
-    Serial.print("IMU I2C address: ");
-    Serial.println(imu.getAddr(), HEX);
+    uint8_t addr = 0x68;
+    if(!I2CSCAN::isI2CExist(addr)) {
+        addr = 0x69;
+        if(!I2CSCAN::isI2CExist(addr)) {
+            Serial.println("Can't find I2C device on addr 0x4A or 0x4B, scanning for all I2C devices and returning");
+            I2CSCAN::scani2cports();
+            signalAssert();
+            return;
+        }
+    }
     // initialize device
-    imu.initialize();
+    imu.initialize(addr);
     if(!imu.testConnection()) {
         Serial.print("Can't communicate with MPU9250, response ");
         Serial.println(imu.getDeviceID(), HEX);

src/mpu9250sensor.cpp

@@ -26,6 +26,8 @@
 #include "udpclient.h"
 #include "defines.h"
 #include "helper_3dmath.h"
+#include <i2cscan.h>
+
 
 #define gscale (250. / 32768.0) * (PI / 180.0) //gyro default 250 LSB per d/s -> rad/s
 // These are the free parameters in the Mahony filter and fusion scheme,
@@ -34,15 +36,34 @@
 #define Kp 10.0
 #define Ki 0.0
 
+CalibrationConfig * calibration;
+
 void get_MPU_scaled();
 void MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz, float deltat);
 
-CalibrationConfig * calibration;
+void signalAssert() {
+    for(int i = 0; i < 200; ++i) {
+        delay(50);
+        digitalWrite(LOADING_LED, LOW);
+        delay(50);
+        digitalWrite(LOADING_LED, HIGH);
+    }
+}
 
 void MPU9250Sensor::motionSetup(DeviceConfig * config) {
     calibration = &config->calibration;
+    uint8_t addr = 0x68;
+    if(!I2CSCAN::isI2CExist(addr)) {
+        addr = 0x69;
+        if(!I2CSCAN::isI2CExist(addr)) {
+            Serial.println("Can't find I2C device on addr 0x4A or 0x4B, scanning for all I2C devices and returning");
+            I2CSCAN::scani2cports();
+            signalAssert();
+            return;
+        }
+    }
     // initialize device
-    imu.initialize();
+    imu.initialize(addr);
     if(!imu.testConnection()) {
         Serial.print("Can't communicate with MPU9250, response ");
         Serial.println(imu.getDeviceID(), HEX);

src/sensor.h

@@ -62,7 +62,7 @@ class MPUSensor : public Sensor {
         MPUSensor() = default;
         ~MPUSensor() override = default;
     protected:
-        MPU9250 imu {MPU9250(MPU9250_ADDRESS_AD0_HIGH)};
+        MPU9250 imu {};
         float q[4] {1.0, 0.0, 0.0, 0.0};
 };