Hallo,
ich nutze zur Zeit einen Sketch aus einem älteren Beitrag (MPU6050 Winkel anzeigen - Deutsch - Arduino Forum) mit dem man die Winkel der X und Y-Achse ausgeben kann.
Hier der Sketch:
#include <Wire.h>
#include "Kalman.h" // Source: https://github.com/TKJElectronics/KalmanFilter
Kalman kalmanX; // Create the Kalman instances
Kalman kalmanY;
/* IMU Data */
int16_t accX, accY, accZ;
int16_t gyroX, gyroY, gyroZ;
double accXangle, accYangle; // Angle calculate using the accelerometer
double gyroXangle, gyroYangle; // Angle calculate using the gyro
double kalAngleX, kalAngleY; // Calculate the angle using a Kalman filter
uint32_t timer;
uint8_t i2cData[14]; // Buffer for I2C data
void setup() {
Serial.begin(115200);
Wire.begin();
TWBR = ((F_CPU / 400000L) - 16) / 2; // Set I2C frequency to 400kHz
i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz
i2cData[1] = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling
i2cData[2] = 0x00; // Set Gyro Full Scale Range to ±250deg/s
i2cData[3] = 0x00; // Set Accelerometer Full Scale Range to ±2g
while (i2cWrite(0x19, i2cData, 4, false)); // Write to all four registers at once
while (i2cWrite(0x6B, 0x01, true)); // PLL with X axis gyroscope reference and disable sleep mode
while (i2cRead(0x75, i2cData, 1));
if (i2cData[0] != 0x68) { // Read "WHO_AM_I" register
Serial.print(F("Error reading sensor"));
while (1);
}
delay(100); // Wait for sensor to stabilize
/* Set kalman and gyro starting angle */
while (i2cRead(0x3B, i2cData, 6));
accX = ((i2cData[0] << 8) | i2cData[1]);
accY = ((i2cData[2] << 8) | i2cData[3]);
accZ = ((i2cData[4] << 8) | i2cData[5]);
// atan2 outputs the value of -? to ? (radians) - see http://en.wikipedia.org/wiki/Atan2
// We then convert it to 0 to 2? and then from radians to degrees
accYangle = (atan2(accX, accZ) + PI) * RAD_TO_DEG;
accXangle = (atan2(accY, accZ) + PI) * RAD_TO_DEG;
kalmanX.setAngle(accXangle); // Set starting angle
kalmanY.setAngle(accYangle);
gyroXangle = accXangle;
gyroYangle = accYangle;
timer = micros();
}
void loop() {
/* Update all the values */
while (i2cRead(0x3B, i2cData, 14));
accX = ((i2cData[0] << 8) | i2cData[1]);
accY = ((i2cData[2] << 8) | i2cData[3]);
gyroX = ((i2cData[8] << 8) | i2cData[9]);
gyroY = ((i2cData[10] << 8) | i2cData[11]);
// atan2 outputs the value of -? to ? (radians) - see http://en.wikipedia.org/wiki/Atan2
// We then convert it to 0 to 2? and then from radians to degrees
accXangle = (atan2(accY, accZ) + PI) * RAD_TO_DEG;
accYangle = (atan2(accX, accZ) + PI) * RAD_TO_DEG;
double gyroXrate = (double)gyroX / 131.0;
double gyroYrate = -((double)gyroY / 131.0);
gyroXangle += gyroXrate * ((double)(micros() - timer) / 1000000); // Calculate gyro angle without any filter
gyroYangle += gyroYrate * ((double)(micros() - timer) / 1000000);
kalAngleX = kalmanX.getAngle(accXangle, gyroXrate, (double)(micros() - timer) / 1000000); // Calculate the angle using a Kalman filter
kalAngleY = kalmanY.getAngle(accYangle, gyroYrate, (double)(micros() - timer) / 1000000);
timer = micros();
/* Print Data */
Serial.print(kalAngleX); Serial.print(" ");
Serial.print(kalAngleY); Serial.println();
delay(1);
}
Kann man mit diesem Sketch auch die Beschleunigung der Z-Achse bestimmen?
Ich habe den "accZ" Wert ausgegeben aber da werden in Ruhelage sehr hohe Werte angezeigt:
17100
17068
17008
17088
17024
17152
17204
17112
17208
17060
17164
17132
17164
17100
17168
17088
17136
17024
17292
17136
17108
17148
16936
17204
17068
17092
17172
17240
17152
17052
17008
17108
17248
17060
17148
Könnte mir jemand sagen wie ich diese Werte interpretieren kann? Ich hätte gerne, dass der Sensor mir in Ruhelage einen Wert von 9,81 m/s² ausgibt, weiß aber nicht wie ich das realisieren soll.
Freundliche Grüße
