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 ;-)
Angehängte Dateien:
