MPU6050 (Gy-521) Neigung Steigung mit Madgwick Filter

[Joh W. (Firma: tet) (hampel)]

Hallo,


habe die Raw Daten aus dem Sensor „MPU6050“ gelesen und den Offset 
berechnet. Desweitern habe ich aus den neuen Werten vom MPU6050 die 
G-Werte berechnet. Mein Microcontroller ist CY8C29466-24PXI.
Zu meiner Frage: Wie implementiere ich einen Madgwick Filter?

Hier mein Code:
#define MPU6050_ACCEL_FS_2  0x00 // ACC Auflösung = +-2g
#define MPU6050_GYRO_FS_250 0x00 // GYRO Auflösung = +-250°/s
..
..
#define sampleFreq  1365.0f    (* sample frequency in Hz von 
https://github.com/kriswiner/MPU-6050/wiki/Affordable-9-DoF-Sensor-Fusion 
*)
#define betaDef    0.1f      // 2 * proportional gain
..
..
while (TRUE)
{
MPU6050_getMotion6(&CAX, &CAY, &CAZ, &CGX, &CGY, &CGZ);
AX = ((float)CAX-AXoff)/16384.00;
AY = ((float)CAY-AYoff)/16384.00;
AZ = ((float)CAZ-(AZoff-16384))/16384.00;

GX = ((float)CGX-GXoff)/131.07;
GY = ((float)CGY-GYoff)/131.07;
GZ = ((float)CGZ-GZoff)/131.07;

MadgwickAHRSupdateIMU(GX, GY, GZ, AX, AY, AZ);

yaw = atan2(2*q1*q2-2*q0*q3,2*q0*q0+2*q1*q1-1)*180.0f/PI;
pitch = -1*asin(2.0f *q1*q3+2.0f *q0*q2)*180.0f/PI;
roll = atan2(2.0f *q2*q3-2.0f *q0*q1,2.0f *q0*q0+
2.0f *q3*q3-1)*180.0f/PI;

..
..
}

MadgwickAHRSupdateIMU() gefunden bei 
http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/



Hier meine Ausgabe: Siehe Bilder


Die G-Werte waren/sind einwandfrei, kennt jemand den Grund für die 
komische Ausgabe?

Freue mich über Hinweise ;-)

[Forist (Gast)]

Joh W. schrieb:
> Freue mich über Hinweise ;-)

Du tust allen Betrachtern einen Gefallen, wenn du die Bilder richtig 
drehst, bevor du sie hochlädst.
Ein Screen Shot funktioniert wohl bei deinem Betriebssystem nicht?

Ich fühle mich echt mindestens 30 Jahre zurück versetzt.

[Joh W. (Firma: tet) (hampel)]

gedreht