Ich werde mal ein paar Messungen zum Vergleich machen.
Was verstehst du unter PLL Source? Ich finde es komisch das der einte Gyro praktisch keinen Drift hat und der andere so extrem.
Eine Idee wäre noch, nicht die Fixe Kurve zu verwenden, sondern beim Start den Start Offset und Temperatur speichern und danach anhand der Temperaturdifferenz zum Start und dem Start Offset den korrigierten Offset anhand der Kurve berechnen. Ich hoffe du verstehst was ich meine 
Das Problem ist mir auch erst jetzt aufgefallen mit dem kleinen ACC Anteil (0.001) beim Komplementärfilter, jetzt hat der ACC-Anteil zu wenig "Kraft" den Winkel zu korrigieren und er läuft mir langsam davon wenn der Gyro wegdriftet... bei einem Anteil von 0.02 hatte der ACC genug "Kraft" den Absoluten Winkel zu korrigieren, obwohl der Gyro langsam wegdriftete....
Ja es wäre sehr intresssant einen Vergleich zu haben. Ich mache einfach einmal pro Sekunde einen Mittelwert und zeichne diesen auf
Hier mal ein paar Code Ausschnitte:
Code:
Dim LSB as Byte
Dim MSB as Byte
Dim Gyr_X as Integer
Dim Gyr_Y as Integer
Dim Gyr_Z as Integer
Dim Temperatur as Integer
Dim Gyr_X_Offset as Integer
Dim Gyr_Y_Offset as Integer
Dim Gyr_Z_Offset as Integer
Dim Gyr_X_Offset_Sum as Long
Dim Gyr_Y_Offset_Sum as Long
Dim Gyr_Z_Offset_Sum as Long
Dim i as Word
'Offset berechnen, Mittelwert aus 300 Messungen
Gyr_X_Offset_Sum = 0
Gyr_Y_Offset_Sum = 0
Gyr_Z_Offset_Sum = 0
For i = 1 to 300
gosub MPU6050_READ
Gyr_X_Offset_Sum = Gyr_X_Offset_Sum + Gyr_X
Gyr_Y_Offset_Sum = Gyr_Y_Offset_Sum + Gyr_Y
Gyr_Z_Offset_Sum = Gyr_Z_Offset_Sum + Gyr_Z
next
decr i
Gyr_X_Offset = Gyr_X_Offset_Sum / i
Gyr_Y_Offset = Gyr_Y_Offset_Sum / i
Gyr_Z_Offset = Gyr_Z_Offset_Sum / i
'---------- Beschleunigungssensor auslesen ----------
MPU6050_read:
I2cstart
I2cwbyte &HD0 'Schreibadresse
I2cwbyte &H3B 'Register h43 auswählen
I2cstop
I2cstart
I2cwbyte &HD1 'Leseadresse
I2crbyte Msb , Ack 'ACC X (Register 3B + 3C)
I2crbyte Lsb , Ack
ACC_X = Makeint(Lsb,Msb)
I2crbyte Msb , Ack 'ACC Y (Register 3D + 3E)
I2crbyte Lsb , Ack
ACC_Y = Makeint(Lsb,Msb)
I2crbyte Msb , Ack 'ACC Z (Register 3F + 40)
I2crbyte Lsb , Ack
ACC_Z = Makeint(Lsb,Msb)
I2crbyte Msb , Ack 'Temperatur (Register 41 + 42)
I2crbyte Lsb , Ack
Temperatur = Makeint(Lsb,Msb)
Temperatur = Temperatur / 34
Temperatur = Temperatur + 365
I2crbyte Msb , Ack 'Gyro X (Register 43 + 44)
I2crbyte Lsb , Ack
Gyr_X = Makeint(Lsb,Msb)
I2crbyte Msb , Ack 'Gyro Y (Register 45 + 46)
I2crbyte Lsb , Ack
Gyr_Y = Makeint(Lsb,Msb)
I2crbyte Msb , Ack 'Gyro Z (Register 47 + 48)
I2crbyte Lsb , Nack
Gyr_Z = Makeint(Lsb,Msb)
Return
'---------- Beschleunigungssensor Initialisieren ----------
MPU6050_init:
'Sensor aktivieren
I2cstart
I2cwbyte &HD0 'Schreibadresse
I2cwbyte &H6B 'Register h6B Powermanagement
I2cwbyte &H00 'Wert 0 schreiben
I2cstop
waitms 200
Toggle LED_R
toggle LED_H
'Filter konfigurieren
' ACC Gyro
'Wert Bandbreite Delay Bandbreite Delay
'0 260Hz 0.0ms 256Hz 1.0ms
'1 184Hz 2.0ms 188Hz 1.9ms
'2 94Hz 3.0ms 98Hz 2.8ms
'3 44Hz 4.9ms 42Hz 4.8ms
I2cstart
I2cwbyte &HD0 'Schreibadresse
I2cwbyte &H1A 'Register h1A Filter
I2cwbyte 6 'Wert schreiben
I2cstop
waitms 200
Toggle LED_R
toggle LED_H
'Gyro Bereich festlegen
'Wert Bereich
'0 +/- 250°/s
'8 +/- 500°/s
'16 +/- 1000°/s
'24 +/- 2000°/s
I2cstart
I2cwbyte &HD0 'Schreibadresse
I2cwbyte &H1B 'Register h1B Gyro konfig
I2cwbyte 16 'Wert schreiben
I2cstop
waitms 200
Toggle LED_R
toggle LED_H
'ACC Bereich festlegen
'Wert Bereich
'0 +/- 2g
'8 +/- 4g
'16 +/- 8g
'24 +/- 16g
I2cstart
I2cwbyte &HD0 'Schreibadresse
I2cwbyte &H1C 'Register h1C ACC konfig
I2cwbyte 0 'Wert schreiben
I2cstop
waitms 200
Toggle LED_R
Toggle LED_H
Return
Gruss Bluesmash
Zitieren
Lesezeichen