Passing Instance of Wire to MPU6050

[degdion]

I am currently using this library on an ESP32 (Arduino framework, platformIO) with two MPU6050 connected to my standard I2C interface (PIN21,22). I would like to add two more but as the IDs seem to be limited to 0x68/0x69 I am trying to add another I2C interface (any ESP32 pin can be configured to that) to avoid using a multiplexer but I don't understand how to pass an instance of the newly created TwoWire Object to class MPU6050 to bind the sensors to their corresponding interface. Is this somehow possible? I do understand that Wire creates global variables Wire and Wire1 but again I do not understand how to tell my MPU6050 objects.

Any help would be appreciated. Daniel

[jrowberg]

Hi @degdion,

Unfortunately, this isn't possible with the code as written. The I2Cdev class has a single static instance, and it assumes that you'd use Wire all the time. It's one of the shortcomings of the original architecture. Because of the static nature of the class, there's no straightforward way to refactor it to support multiple Wire objects without requiring a lot of changes in each device library.

Let me think about this and see what I can come up with.

[jrowberg]

Hi @degdion,

New code has been committed--pull and try the latest Arduino I2Cdev and MPU6050 source files. There's a trivial example of the MPU6050 object creation in the raw example:

https://github.com/jrowberg/i2cdevlib/blob/da71c0469cd63316963a9871f732f40d624680b0/Arduino/MPU6050/examples/MPU6050_raw/MPU6050_raw.ino#L51

Everything compiles, but I haven't pulled the requisite hardware out of the drawer(s) to test it on the real thing. Let me know if it works.

[degdion]

Hi @jrowberg

thank you so much for the quick response and implementation. I just tested it and it works for me. Because I am using the DMP (what I forgot to mention) I added the following constructor initialization list to MPU6050_6Axis_MotionApps20.h: public: MPU6050_6Axis_MotionApps20(uint8_t address=MPU6050_DEFAULT_ADDRESS, void *wireObj=0){ MPU6050_Base(address, wireObj); } and it works smoothly when using global instances Wire and Wire1. I tried passing my own instances of TwoWire class to it but that fails. Anyways it solves my issue of using 4 MPU6050 (DMP) on one ESP32 with two separate I2C buses.

Thanks again you are doing great work here. Daniel

[degdion]

Sorry for reopening but after mounting 3 sensors I found out that I am now reading only from one sensor regardless which instance I try. `// join I2C bus (I2Cdev library doesn't do this automatically)

if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE

Wire.begin(SDA_1, SCL_1);
Wire.setClock(400000);
delay(500);
Wire1.begin(SDA_2, SCL_2);
Wire1.setClock(400000);

elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE

Fastwire::setup(400, true);

endif

// init access array sensors.setStorage(storage_array); sensors.push_back(MPU6050(MPU6050_ADDRESS_AD0_HIGH, &Wire)); sensors.push_back(MPU6050(MPU6050_ADDRESS_AD0_LOW, &Wire)); sensors.push_back(MPU6050(MPU6050_ADDRESS_AD0_LOW, &Wire1)); // <--

Serial.begin(115200);

// Start MPU: mpu_setup(0); mpu_setup(1); mpu_setup(2); `

As you can see I am using a vector so setup code for all sensors hasn't changed at all except that I pass index 2. Also readout hasn't been touched as I use a reset fifo and polling function: void loop() { //only send data when connected if(connected) { if (hostListening) { mpu_loop_polling(0); mpu_loop_polling(1); mpu_loop_polling(2); }

in which I generated a Serial print using: Serial.print(bufData.dt);Serial.print(",");Serial.print(bufData.imuNumber);Serial.print(","); Serial.print(bufData.quaternion.w);Serial.print(",");Serial.print(bufData.quaternion.x);Serial.print(",");Serial.print(bufData.quaternion.y);Serial.print(",");Serial.print(bufData.quaternion.z);Serial.print(","); Serial.print(bufData.accelerationLinear.x);Serial.print(",");Serial.print(bufData.accelerationLinear.y);Serial.print(",");Serial.print(bufData.accelerationLinear.z);Serial.print(","); Serial.print(bufData.gyroDmp.x);Serial.print(",");Serial.print(bufData.gyroDmp.y);Serial.print(",");Serial.print(bufData.gyroDmp.z);Serial.print(","); Serial.print(bufData.accelerationRaw.x);Serial.print(",");Serial.print(bufData.accelerationRaw.y);Serial.print(",");Serial.print(bufData.accelerationRaw.z);Serial.print(","); Serial.println(bufData.imuNumber);

( buData is just a storage class and used like this: sensors[imu_num].dmpGetLinearAccel(&bufData.accelerationLinear, &bufData.accelerationRaw, &gravity); //aaReal )

which gave me:

imu No: 1 timestamp: 37991 30,0,0.99,0.14,-0.03,-0.01,111,607,-6121,0,0,-1,111,607,2071,0 imu No: 2 timestamp: 38001 31,1,0.99,0.14,-0.03,-0.01,112,610,-6128,0,-1,-1,112,610,2064,1 imu No: 0 timestamp: 38011 30,2,0.99,0.14,-0.03,-0.01,112,613,-6136,0,0,0,112,613,2056,2

So sensors No.0, No1 (Wire) output the reading of sensor No.2 (Wire1) with some added noise (which made me believe everything is working in the first place). (sensor No.2 not calibrated! So accelerationRaw and quaternion are not plausible )

Do you know what's going on here?

Daniel

[jrowberg]

Hi @degdion,

I neglected to test the custom-Wire construction--my mistake. For reasons I'm not yet 100% certain of (probably strange interplay between initializer lists and in-function initialization), the constructor you added to the MotionApps20 subclass wasn't properly passing through the custom Wire object pointer, so wireObj remained at the default 0 value and the read/write functions fell back on the Wire object as a result. I've just pushed a modification to all three of the DMP-specific subclass header files that corrects this, as far as I have tested. Try it now and see what happens.

[degdion]

Hi @jrowberg,

my bad I should have mentioned that I am using MPU6050_6Axis_MotionApps20 in the first place. I just tested your latest modification and it works perfectly fine now.

Thanks again, Daniel

[jrowberg]

Great news! Thanks for confirming, and finally prompting me to make this happen on the old codebase.

[degdion]

@jrowberg I don't want to make this a never ending story but I rarely still encounter the issue of one sensor outputting readings of another sensor. It is now pretty hard to reproduce as timespan till it happens varies a lot ( from 6min ... ~30min). I generated the print you saw in my earlier post again and as you can see two sensors are reporting now the same readings (readings were valid for about 20min before that):

40,3,0.62,-0.03,0.79,0.01,-7827,-171,-10105,-1,0,-1,-7827,-171,-1913,3 36,0,0.62,-0.03,0.79,0.01,-7827,-171,-10105,-1,0,-1,-7827,-171,-1913,0 38,1,0.84,0.52,0.00,-0.14,-1201,6902,-4541,0,0,-1,-1201,6902,3651,1 35,2,0.84,0.52,0.00,-0.14,-1201,6902,-4541,0,0,-1,-1201,6902,3651,2

The last number represents the sensor No. . I haven't found a tendency of a specific sensor failing first yet, but if I wait long enough all sensors show the same reading in the end. I tried just restarting if this happens, but than dmpinit fails (1) permamently until I switch power on and off.

[jrowberg]

Hmm. Can you try running your code with only two IMUs, with the following configurations:

• Test 1: one IMU on Wire, one IMU on Wire1
• Test 2: two IMUs on WIre, no IMUs on Wire1

Although I believe Arduino Wire code is all blocking during transaction execution, some buffer data has to be getting overwritten somewhere. If the I2C buses were competing on a hardware level, the data would be way more scrambled. If you have a logic analyzer that you can capture both sets of SDA/SCL lines with, that would be helpful also, but if not, you can at least start with the above tests.

[degdion]

Hi @jrowberg , sorry for the late response I did the testing you suggested but to isolate the problem I setup a new hardware (my old HW was kind of too integrated), because what I saw confused me. So now I am using an esp32 with just two MPU6050 connected via a 10cm cable soldered directly on the pins on both sides

Test 1: one IMU on Wire, one IMU on Wire1

I can not see the replication I mentioned earlier anymore, but what happens now is that at some point (like after 5-6min) dmp fails to reinit (return value 1). I do a reinit, so initialize() and dmpInitialize(), when fifobuffer reading (I cannot use interrupts so I am polling) gets into timeout. So this configuration doesn‘t work for me at all, because when dmp status returns 1 I reinit till it works which blocks my application at the moment.

Test 2: two IMUs on WIre, no IMUs on Wire1

The issue remains as described above. After 10-15min one sensor outputs the exact same readings as the other. I do NOT see any pattern like sensor 1 is always reporting sensor 2 reading. What can acually happen is that timeout hits (I cannot figure out on which sensor. The one with valid readings or the one replicating.) and then it works again (for a few minutes).

[degdion]

Hi @jrowberg ,

I tried to isolate the issue by setting up a new hardware again. This time I use a multiplexer and just the TwoWire instance Wire (noWire1 in my code). I have 4 MPU6050 attached to the multiplexer: imu0 - 0x68 imu1 - 0x68 imu2 - 0x69 imu3 - 0x68

Again I am polling each fifo (mostly for debugging convenience) with the following code:

void mpu_loop_polling(int imu_num)
{
    // orientation/motion vars
  Quaternion q;           // [w, x, y, z]         quaternion container
  VectorFloat gravity;    // [x, y, z]            gravity vector
  uint8_t fifoBuffer[FIFO_BUFFER_SIZE];

  #ifdef DEBUG
  int start = millis();
  Serial.print("polling imu#: ");Serial.print(imu_num);Serial.print(" timestamp: ");Serial.println(millis());
  Serial.print("print overhead time pol around: ");Serial.println(millis()-start);
  #endif

  setTCAChannel(imu_num);
  sensors[imu_num].resetFIFO();

  // get current FIFO count
  fifoCount = sensors[imu_num].getFIFOCount();

  // wait for correct available data length, should be a VERY short wait
  int pt = millis();
  while (fifoCount < packetSize) 
  {
    fifoCount = sensors[imu_num].getFIFOCount();
    if (millis()-pt > pollingTimeout)
    {
      Serial.print("polling imu#: ");Serial.print(imu_num);Serial.println(" Timeout ! ");
      Serial.println( "Reinitializing IMU!");mpu_setup(imu_num);
      // Serial.println( "Restarting ESP32 !");ESP.restart();
      return;
    }
  }
  // read a packet from FIFO
  sensors[imu_num].getFIFOBytes(fifoBuffer, packetSize);

  MpuData bufData;
  bufData.imuNumber = imu_num;
  sensors[imu_num].dmpGetQuaternion(&bufData.quaternion, fifoBuffer);
  sensors[imu_num].dmpGetGyro(&bufData.gyroDmp, fifoBuffer);
  sensors[imu_num].dmpGetAccel(&bufData.accelerationRaw, fifoBuffer); //aa
  sensors[imu_num].dmpGetGravity(&gravity, &q);
  sensors[imu_num].dmpGetLinearAccel(&bufData.accelerationLinear, &bufData.accelerationRaw, &gravity); //aaReal
  //sensors[imu_num].dmpGetLinearAccelInWorld(&bufData.accelerationWorld, &bufData.accelerationLinear, &q);
  if (statusCS[imu_num]){
    Serial.print("critical buffer overflow before DMP Reading of #");Serial.print(bufData.imuNumber);Serial.print(" timestamp: ");Serial.println(millis());
    Serial.print("acc z reading: ");Serial.print(bufData.accelerationRaw.z);
    }

  bufData.dt = (int)millis()-lastTimeStamp[imu_num];
  bufData.timestamp = (int)millis();
  lastTimeStamp[imu_num] = bufData.timestamp;

  sensors[imu_num].resetFIFO();
  //sensors[0].resetFIFO();sensors[1].resetFIFO();sensors[2].resetFIFO();sensors[3].resetFIFO();

  bool unplausible = bufData.quaternion.w == 0 && bufData.quaternion.x == 0 && bufData.quaternion.y == 0 && bufData.quaternion.z == 0;

  if(xSemaphoreTake(binSemaphore_A, semaphoreWait) )
  {
    if (sendBuffer.isFull())
    {
      sendBuffer.clear();
      Serial.print("Clearing Send buffer overflow! IMU #");Serial.print(imu_num);Serial.print(" timestamp: ");Serial.println(millis());
      statusCounter[imu_num]++;
      if (statusCounter[imu_num]>10){statusCS[imu_num] = true;}
    }
    // commit data:
    if (connected && !unplausible)
    {
      sendBuffer.push(bufData);
      if (digitalRead(25)== HIGH)
      {
        int start =millis();
        Serial.print(bufData.dt);Serial.print(",");Serial.print(bufData.imuNumber);Serial.print(",");
        Serial.print(bufData.quaternion.w);Serial.print(",");Serial.print(bufData.quaternion.x);Serial.print(",");Serial.print(bufData.quaternion.y);Serial.print(",");Serial.print(bufData.quaternion.z);Serial.print(",");
        Serial.print(bufData.accelerationLinear.x);Serial.print(",");Serial.print(bufData.accelerationLinear.y);Serial.print(",");Serial.print(bufData.accelerationLinear.z);Serial.print(",");
        Serial.print(bufData.gyroDmp.x);Serial.print(",");Serial.print(bufData.gyroDmp.y);Serial.print(",");Serial.print(bufData.gyroDmp.z);Serial.print(",");
        Serial.print(bufData.accelerationRaw.x);Serial.print(",");Serial.print(bufData.accelerationRaw.y);Serial.print(",");Serial.print(bufData.accelerationRaw.z);Serial.print(",");
        Serial.println(bufData.imuNumber);
      }
    } 
    xSemaphoreGive(binSemaphore_A);
  }
  // is reading unplausible?
  if (unplausible)
  {
    statusCounter[imu_num] = statusCounter[imu_num]+ bufData.dt;
    if (statusCounter[imu_num] > restartAbility)
    {
      Serial.print("Sensor readings of imu#: ");Serial.print(imu_num);Serial.println(" unplausible ! ");
      Serial.println( "Reinitializing IMU!");mpu_setup(imu_num);
    }
  }
  else
  {
    statusCounter[imu_num]=0;
  }
}

I streamed the sensor readings to ROS to visualize 4 poses but after some time some sensors start again outputting readings of other sensors like in the following case:

(To decode the serial prints precisely see my post above. It is : dt(imu-cycletime), imuNumber, Quaternion, accLinear, Gyro, AccRaw, imuNumber) 4 poses everything works:

70,0,1.00,0.01,-0.06,0.02,890,194,-218,-1,-1,0,890,194,7974,0 60,2,0.99,0.03,-0.02,-0.12,239,563,-103,0,-1,-1,239,563,8089,2 60,3,0.03,-0.96,-0.28,-0.00,175,-621,-16798,-1,-1,-1,175,-621,-8606,3 70,1,0.20,0.98,0.07,0.01,-33,3606,-16677,-1,0,-1,-33,3606,-8485,1 60,2,0.99,0.03,-0.02,-0.12,236,557,-110,0,-1,-1,236,557,8082,2 62,3,0.03,-0.96,-0.28,-0.00,176,-612,-16800,-1,-1,-1,176,-612,-8608,3 61,0,1.00,0.01,-0.06,0.02,878,198,-214,-1,-1,0,878,198,7978,0 60,1,0.20,0.98,0.07,0.01,-34,3618,-16670,-1,0,-1,-34,3618,-8478,1 61,2,0.99,0.03,-0.02,-0.12,235,559,-107,0,-1,-1,235,559,8085,2

3 poses left: (after 4min)

49,0,1.00,0.02,-0.05,0.05,891,229,-207,0,-1,0,891,229,7985,0 42,1,0.20,0.98,-0.01,0.00,67,3634,-16670,0,-1,-1,67,3634,-8478,1 40,2,0.69,-0.26,-0.67,-0.03,8165,-2756,-8570,0,0,0,8165,-2756,-378,2 44,3,0.69,-0.26,-0.67,-0.03,8165,-2756,-8570,0,0,0,8165,-2756,-378,3 41,0,1.00,0.02,-0.05,0.05,883,220,-203,0,-1,0,883,220,7989,0 40,1,0.20,0.98,-0.01,0.00,71,3626,-16672,0,0,-1,71,3626,-8480,1 38,3,0.69,-0.26,-0.67,-0.03,8154,-2756,-8556,0,-1,0,8154,-2756,-364,3 40,2,0.69,-0.26,-0.67,-0.03,8152,-2766,-8547,-1,0,0,8152,-2766,-355,2 38,3,0.69,-0.26,-0.67,-0.03,8152,-2766,-8547,-1,0,0,8152,-2766,-355,3

imu2 still valid ← got replicated by imu3

2 poses left: (after ~9min)

30,3,0.20,0.98,-0.02,-0.00,39,3649,-16676,-1,0,0,39,3649,-8484,3 30,1,0.20,0.98,-0.02,-0.00,39,3640,-16667,0,-1,-1,39,3640,-8475,1 30,2,0.20,0.98,-0.02,-0.00,39,3640,-16667,0,-1,-1,39,3640,-8475,2 30,3,0.20,0.98,-0.02,-0.00,39,3640,-16667,0,-1,-1,39,3640,-8475,3 30,0,1.00,0.02,-0.05,0.05,884,202,-203,0,-1,0,884,202,7989,0 30,1,0.20,0.98,-0.02,-0.00,47,3636,-16665,-1,0,-1,47,3636,-8473,1 30,2,0.20,0.98,-0.02,-0.00,47,3636,-16665,-1,0,-1,47,3636,-8473,2 31,3,0.20,0.98,-0.02,-0.00,47,3636,-16665,-1,0,-1,47,3636,-8473,3 31,0,1.00,0.02,-0.05,0.05,879,201,-199,0,-1,0,879,201,7993,0 30,1,0.20,0.98,-0.02,-0.00,46,3632,-16675,-1,-1,-1,46,3632,-8483,1 32,2,0.20,0.98,-0.02,-0.00,46,3632,-16675,-1,-1,-1,46,3632,-8483,2 32,3,0.20,0.98,-0.02,-0.00,46,3632,-16675,-1,-1,-1,46,3632,-8483,3 30,0,1.00,0.02,-0.05,0.05,887,209,-199,1,-1,-1,887,209,7993,0

imu1 and imu0 still valid but imu1 got replicated by imu2 and imu3

What I see is that dt(imu-cycletime) so the sum of millis() it takes for all polled sensors decreases which should not be the case. Looking at a scope shows me for imu2 ( it is the only 0x69 ) which is replicating imu1, that a NACK is present.

Do you know what is going on here? If I can be of any help let me know I am going to dig deeper into it tomorrow morning for a few hours.

Thanks, Daniel

(Sorry I don't know why my code formatting doesn't work)

[PhilippMolitor]

I just came across this issue as I was developing an IMU driver with I2Cdev.h. Using Wire is not possible due to my board being made in a way that only allows me to use specific SDA/SCL pins, which are assigned to Wire1. Really hope this library will support using a custom TwoWire instance in the future.

Edit: I just found the wireObj parameter, seems like there is a workaround already. This issue should maybe be closed/updated then?

[Sanassah]

Hi @jrowberg,

my bad I should have mentioned that I am using MPU6050_6Axis_MotionApps20 in the first place. I just tested your latest modification and it works perfectly fine now.

Thanks again, Daniel

Hi could you please output your code so I could use it? Thanks a lot