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