Data Problem with Multiple MPU6050 and Arduino Nano

Question

I've been working on a recent project which involves multiple IMU's(Specifically 5 MPU6050's, GY-521 Breakout board). Everything works fine, from angle calculation to dealing with gyroscopic drift for TWO sensors. The problem comes when I add in a third sensor.

Basically what happens is, when printing out the X/Y values(on the Serial Monitor) of two sensors(S1 and S2) they work out fine, but when a third sensor(S3) is included, the angle values from S1 completely mirror for the S3 values. I performed multiple tests(Shorts, Software/Hardware, Swapping/Switching sensors, Swapping Standby and Reading address, etc.) yet the results are the same.

When I run the function called "ThirdGyro" even without printing it, is when things go haywire. I'm using the "CD4051" Demux to switch between the sensors.

The MPU6050 has 2 addresses in which it can be read on, 0x68 - Default, and 0x69. The way I do it for 3 sensors, is by reading the values from the sensor which has its AD0 pin set HIGH,(0x69) which I've set as the reading address (0x68 as standby/not read). With the common output(3) set high on the CD4051, and all ADO pins of the IMU connected to their respective I/O's, by Demuxing I can isolate each sensor and have it read.

I didn't run the SCL and SDA pins of the IMU, through the Demux just cause I thought it was a hassle. Also guys this is my first question :)

//Necessary Libraries
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"

MPU6050 accelgyroSB(0x68); // SB - Standby Gyro, Not Read
MPU6050 accelgyroR(0x69); // R - Reading gyro, AD0 = H then being Read

//Global Gyro/Accel Variables for RAW DATA
int16_t gx, gy, gz, gsx, gsy, gsz, ax, ay, az, asx, asy, asz;

double timeStep, time, timePrev;
double arx, ary, arz, grx, gry, grz, rx, ry, rz;
int i;

//Filtered Gyro/Accel Variables for Sensors: r[Axis] [Sensor#]
int rx1, rx2, rx3, ry1, ry2, ry3, rz1, rz2, rz3;


void setup(){
  Wire.begin(); //Join 12c bus with lib
  Serial.begin(9600);

  accelgyroR.initialize();
  accelgyroSB.initialize();

  //Initialization of Gyroscope
  time = millis();

  i = 1;

  pinMode(3, OUTPUT); //Select Channel Input A
  pinMode(4, OUTPUT); //Select Channel Input B
  pinMode(5, OUTPUT); //Select Channel Input C

  pinMode(6, OUTPUT); //Common Output for HIGH 
  digitalWrite(6, HIGH);

  //Connection Check
  Serial.println("Preliminary Connection Check..");
  Serial.println(accelgyroR.testConnection() ? "MPU6050 #1 connection 
  successful" : "Connection Failed");
  }


  void AngleCalc(){ //Universal Angle Calculation with Complimentary Filter

  timePrev = time;
  time = millis();
  timeStep = (time - timePrev) / 1000; //Step in seconds

  //Gathering Raw Gyroscope Data of Selected Gyro
  accelgyroR.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

  //Scaling Raw data with 131(Gyro) and 2048(Accelerometer) scale factors  
  from Datasheet, s=scaled

  gsx = gx / 131;
  gsy = gy / 131;
  gsz = gz / 131;
  asx = ax / 2048;
  asy = ay / 2048;
  asz = az / 2048;

  //Accelerometer angle calculation
  arx = (180/3.141592) * atan(asx / sqrt(square(asy) + square(asz)));
  ary = (180/3.141592) * atan(asy / sqrt(square(asx) + square(asz)));
  arz = (180/3.141592) * atan(sqrt(square(asy) + square(asx)) / asz);

  if (i == 1){
    grx = arx;
    gry = ary;
    grz = arz;
    }
  else{
    grx = grx + (timeStep * gsx);
    gry = gry + (timeStep * gsy);
    grz = grz + (timeStep * gsz);
    }
  //Cocktail of Gyro and Accelerometer data ratioed ;)
  rx = (0.96 * arx) + (0.04 * grx);
  ry = (0.96 * ary) + (0.04 * gry);
  rz = (0.96 * arz) + (0.04 * grz);

  }

void FirstGyro(){
  digitalWrite(3, LOW); //A           ADDRESS : 000
  digitalWrite(4, LOW);  //B
  digitalWrite(5, LOW);  //C
  AngleCalc();
  rx1 = rx;
  ry1 = ry; 
  rz1 = rz;
  delay(20);
}

void SecondGyro(){
  digitalWrite(3, HIGH); //A          ADDRESS : 001
  digitalWrite(4, LOW);  //B
  digitalWrite(5, LOW);  //C
  AngleCalc();
  rx2 = rx;
  ry2 = ry; 
  rz2 = rz;
  delay(20);
}

void ThirdGyro(){
  digitalWrite(3, LOW);  //A          ADDRESS : 010
  digitalWrite(4, HIGH); //B
  digitalWrite(5, LOW);  //C 
  AngleCalc();
  rx3 = rx;
  ry3 = ry;
  rz3 = rz;
  delay(20);
  }

void loop(){

//MUXING THE AD0 PIN SERVES AS IDENTIFYING UNIQUE SENSORS
//BELOW CODE REFERS TO ABOVE RESPECTIVE FUNCTIONS OF RESPECTIVE SENSORS

  FirstGyro();
  delay(10);
  SecondGyro();
  delay(10);
  ThirdGyro();

  Serial.println("");
  Serial.print(ry1); Serial.print("\t");
  Serial.print(ry2); Serial.print("\t");
  Serial.print(ry3); 
  //Print out filtered data

  }  

^{simulate this circuit – Schematic created using CircuitLab}

Answer 1

Your software implies that Arduino pins 3, 4 and 5 are connected to the CD4051, but your schematic does not show this. Are you sure that those connections are correct? Can you verify that the three AD0 lines are switching in the way that you intend? The CD4051 is an analog switch, which means that the deselected pins are open-circuit. Are you using pull-down resistors to make sure that the AD0 lines are seen as logic "0" when not selected?

The pulldown resistor needs to be small enough so that it can overcome any leakage currents that are trying to pull the pin high -- and also discharge the node capacitance reasonably quickly -- but not so small that the drive coming through the CD4051 can't overcome the resistor. For CMOS logic, values in the range of 10K to 100K are usually good.

^{simulate this circuit – Schematic created using CircuitLab}

As an alternative, you might consider switching to a decoder that doesn't tristate its outputs, such as a 74HC138. This particular chip has active-low outputs, which means that you would have to switch which address you consider to be the "active" address in your firmware.

There's one more side issue that I can't resist commenting on. You have written your software using global variables exclusively. I'm guessing that your programming background is mainly in the BASIC language. This is OK for small projects, but will get you into trouble with anything more complex. In the C language and its derivatives, you'll want to learn how to use local variables within functions, and how to use parameters and return values to pass data into and out of functions without using global variables.