Arduino MPU 6050

Hallo zusammen.

Ich bin gerade daran für meine Maturarbeit einen Quadrocopter zu bauen. Das meiste Funktioniert recht gut ich habe jedoch Probleme mit der Berechnung der Z Achse vom MPU 6050 von Arduino.

Hier ist der Code, bei dem ich die Achsen berechne. Die X und Y Achse funktioniert tadellos jedoch bekomme ich für den Z Achsen Wert die genau gleiche Zahl wie für die X-Achse. Ich hoffe es kann mir jemand bei meinem Problem weiterhelfen.

// set up time for integration


  timePrev = time;


  time = millis();


  timeStep = (time - timePrev) / 1000; // time-step in s





  // collect readings


  accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);





  // apply gyro scale from datasheet


  gsx = gx/gyroScale;   gsy = gy/gyroScale;   gsz = gz/gyroScale;





  // calculate accelerometer angles


  arx = (180/3.141592) * atan(ax / sqrt(square(ay) + square(az))); 


  ary = (180/3.141592) * atan(ay / sqrt(square(ax) + square(az)));


  arz = (180/3.141592) * atan(sqrt(square(ax) + square(ay)) / az);





  // set initial values equal to accel values


  if (i == 1) {


    grx = arx;


    gry = ary;


    grz = arz;


  }


  // integrate to find the gyro angle


  else{


    grx = grx + (timeStep * gsx);


    gry = gry + (timeStep * gsy);


    grz = grz + (timeStep * gsz);


  }  


  // apply filter


  rx = (0.96 * arx) + (0.04 * grx);


  ry = (0.96 * ary) + (0.04 * gry);


  rz = (0.96 * arz) + (0.04 * grz);


}

Vielen Dank und Grüsse
Rouben