Datenproblem mit mehreren MPU6050 und Arduino Nano

Ich habe an einem aktuellen Projekt gearbeitet, das mehrere IMUs umfasst (insbesondere 5 MPU6050, GY-521 Breakout-Board). Alles funktioniert gut, von der Winkelberechnung bis zum Umgang mit Kreiseldrift für ZWEI Sensoren. Das Problem tritt auf, wenn ich einen dritten Sensor hinzufüge.

Grundsätzlich passiert Folgendes: Beim Ausdrucken der X / Y-Werte (auf dem seriellen Monitor) von zwei Sensoren (S1 und S2) funktionieren sie einwandfrei, aber wenn ein dritter Sensor (S3) enthalten ist, spiegeln sich die Winkelwerte von S1 vollständig wider für die S3-Werte. Ich habe mehrere Tests durchgeführt (Kurzschlüsse, Software/Hardware, Sensoren tauschen/umschalten, Standby tauschen und Adresse lesen usw.), aber die Ergebnisse sind dieselben.

Wenn ich die Funktion namens "ThirdGyro" ausführe, auch ohne sie zu drucken, geht es drunter und drüber. Ich verwende den "CD4051" Demux, um zwischen den Sensoren umzuschalten.

Die MPU6050 hat 2 Adressen, an denen sie gelesen werden kann, 0x68 - Default und 0x69. Die Art und Weise, wie ich es für 3 Sensoren mache, besteht darin, die Werte von dem Sensor zu lesen, dessen AD0-Pin auf HIGH gesetzt ist (0x69), den ich als Leseadresse eingestellt habe (0x68 als Standby / nicht gelesen). Wenn der gemeinsame Ausgang (3) am CD4051 hoch eingestellt ist und alle ADO-Pins der IMU mit ihren jeweiligen E / A verbunden sind, kann ich durch Demuxing jeden Sensor isolieren und lesen lassen.

Ich habe die SCL- und SDA-Pins der IMU nicht durch den Demux geführt, nur weil ich dachte, es wäre ein Ärger. Auch Leute, das ist meine erste Frage :)

//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

  }  

^{Simulieren Sie diese Schaltung – Mit CircuitLab erstellter Schaltplan}