Salut René,
der gyro liefert sehr sprunghafte Ergebniss, so dass man sie glätten muss:
Hier der Auszug aus meinem derzeitigen testcode:
MPU6050 accelgyro;
int16_t ax, ay, az;
int16_t gx, gy, gz;
#define Gry_offset 0 //The offset of the gyro
#define Gyr_Gain 16.348
#define Angle_offset 0 // The offset of the accelerator
float Angle_Delta, Angle_Recursive, Angle_Confidence;
void Filter()
{
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
//Angle_Raw = (atan2(ay, az) * 180 / pi + Angle_offset);
Angle_Raw = (atan2(az, ay) * 180 / pi + Angle_offset);
omega = gx / Gyr_Gain + Gry_offset;
// Filters datas to get the real move
unsigned long now = micros();
float dt = (now - preTime) * 0.000001;
preTime = now;
float K = 0.8;
float A = K / (K + dt);
Angle_Delta = (Angle_Raw - Angle_Filtered) * 0.64;
Angle_Recursive = Angle_Delta * dt + Angle_Recursive;
Angle_Confidence = Angle_Recursive + (Angle_Raw - Angle_Filtered) * 1.6 + omega;
Angle_Filtered = Angle_Confidence * dt + Angle_Filtered;
}
In der Setuproutine lasse ich den Filter mindestens 100 mal laufen und sehe bei meinem Witty, wie sich der Winkel stabilisiert. Wenn sich der Robot aufstellt, mache ich das im loop dann auch.
Ich hoffe, das spart Dir einiges an Zeit.
Ciao, Mathias