From 31fac76b8de00475b1e535d97329b0eeb8a89c30 Mon Sep 17 00:00:00 2001 From: Eiren Rain Date: Tue, 16 Mar 2021 10:40:20 +0300 Subject: [PATCH] Check different I2C addresses when connecting to IMU --- lib/i2cscan/i2cscan.cpp | 8 ++++++++ lib/i2cscan/i2cscan.h | 1 + lib/mpu9250/MPU9250.cpp | 21 ++++++--------------- lib/mpu9250/MPU9250.h | 3 +-- src/bno080sensor.cpp | 30 +++++++++++++++++++++--------- src/defines.h | 5 ----- src/main.cpp | 4 ++-- src/mpu6050sensor.cpp | 24 +++++++++++++++++++++--- src/mpu9250sensor.cpp | 25 +++++++++++++++++++++++-- src/sensor.h | 2 +- 10 files changed, 84 insertions(+), 39 deletions(-) diff --git a/lib/i2cscan/i2cscan.cpp b/lib/i2cscan/i2cscan.cpp index 7535533..cf210d6 100644 --- a/lib/i2cscan/i2cscan.cpp +++ b/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; + } } diff --git a/lib/i2cscan/i2cscan.h b/lib/i2cscan/i2cscan.h index 16ce560..0a423d0 100644 --- a/lib/i2cscan/i2cscan.h +++ b/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_ \ No newline at end of file diff --git a/lib/mpu9250/MPU9250.cpp b/lib/mpu9250/MPU9250.cpp index 96fd1ac..2938cc1 100644 --- a/lib/mpu9250/MPU9250.cpp +++ b/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 diff --git a/lib/mpu9250/MPU9250.h b/lib/mpu9250/MPU9250.h index 0dd2901..af836d2 100644 --- a/lib/mpu9250/MPU9250.h +++ b/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(); diff --git a/src/bno080sensor.cpp b/src/bno080sensor.cpp index c03a325..3d4fc0e 100644 --- a/src/bno080sensor.cpp +++ b/src/bno080sensor.cpp @@ -27,22 +27,34 @@ #include "defines.h" #include +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 "); diff --git a/src/defines.h b/src/defines.h index b35eb08..d803b42 100644 --- a/src/defines.h +++ b/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 diff --git a/src/main.cpp b/src/main.cpp index adaab72..b6cd5d5 100644 --- a/src/main.cpp +++ b/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); diff --git a/src/mpu6050sensor.cpp b/src/mpu6050sensor.cpp index 0667d00..9956509 100644 --- a/src/mpu6050sensor.cpp +++ b/src/mpu6050sensor.cpp @@ -24,14 +24,32 @@ #include "MPU9250.h" #include "sensor.h" #include "udpclient.h" +#include + +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); diff --git a/src/mpu9250sensor.cpp b/src/mpu9250sensor.cpp index efa543f..6470e95 100644 --- a/src/mpu9250sensor.cpp +++ b/src/mpu9250sensor.cpp @@ -26,6 +26,8 @@ #include "udpclient.h" #include "defines.h" #include "helper_3dmath.h" +#include + #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); diff --git a/src/sensor.h b/src/sensor.h index 47cb4ca..8f2ea22 100644 --- a/src/sensor.h +++ b/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}; };