Gyro-Signal (und RC-Empfänger) filtern

[Che Guevara]

So, ist ein bisschen später geworden, aber die Platine ist fertig und bestückt! Auf ihr befinden sich ein LM317 (SMD), 4 Wiederstande, 2 Kondensatoren, eine Wii Motion Plus und eine Nunchuk.
Ich kann beide Sensoren direkt hintereinander mittels I2C auslesen; beim WM+ bekomme ich akzeptable Werte (ich shifte die tatsächlichen Werte um 2 nach rechts, sprich ich teile durch 4), somit ist kein Drift mehr vorhanden (bzw. er ist normalweise schon so klein, dass er jetzt wegfällt). Hab zwar schon öfter gelesen, dass die WM+ ziemlich viel Drift hat, das kann ich jedoch nicht(!) bestätigen!
Leider verstehe ich den Nunchuk nicht so ganz... Einer der Werte (acc-z) steht für mich in keinem Verhältnis zu irgendeiner Bewegung der Platine?! ... Der Wert verändert sich zwar manchmal, aber ich wei0 nicht, wie genau ich die Platine bewegen muss, damit das passiert? Wo ist beim Nunchuk die Z-Achse?

Nun aber zu meiner eigentlichen Frage:
Wie kann ich die ACC-Daten mit den Gyros verrechnen? Irgendwo hab ich in letzer Zeit mal gelesen, dass man mit einem Kalman-Filter diese beiden Senoren kombinieren kann?! Hier im Forum gibt es ja bereits einen BASCOM-Code für einen Kalman, jedoch wei0 ich nicht, wo ich da die ACC-Werte einsetzen soll. Wäre toll, wenn mir das jemand erklären könnte?!
Hier mal mein momentaner Code:

$regfile = "m32def.dat"
$crystal = 16000000
$framesize = 150
$hwstack = 150
$swstack = 150


Declare Sub Pid_regulator()

Declare Sub Wmp_init()
Declare Sub Send_zero()
Declare Sub Read_wmp_data()
Declare Sub Set_wmp_offset()

Declare Sub Nunchuk_init()
Declare Sub Nunchuk_read()
Declare Sub Set_nunchuk_offset()

Declare Function Tastenabfrage() As Byte



Config Adc = Single , Prescaler = Auto
Start Adc

Config Pina.7 = Input
Porta.7 = 1


Dim Taste As Byte
Dim _direction As Byte
_direction = 1


$lib "I2C_TWI.LBX"
Config Scl = Portc.0
Config Sda = Portc.1
Config Twi = 100000
I2cinit


Config Lcd = 16 * 2
Config Lcdpin = Pin , Db4 = Portb.0 , Db5 = Portb.1 , Db6 = Portb.2 , Db7 = Portb.3 , E = Portb.4 , Rs = Portb.5
Config Lcdbus = 4
Initlcd
Cursor Off
Cls

Locate 1 , 1
Lcd "*WM+ // NUNCHUK*"


Dim Nunchuk_buffer(6) As Byte
Dim Wmplus_buffer(6) As Byte


Dim Yaw As Word
Dim Yaw0 As Byte At Yaw + 1 Overlay
Dim Yaw1 As Byte At Yaw Overlay

Dim Roll As Word
Dim Roll0 As Byte At Roll + 1 Overlay
Dim Roll1 As Byte At Roll Overlay

Dim Pitch As Word
Dim Pitch0 As Byte At Pitch + 1 Overlay
Dim Pitch1 As Byte At Pitch Overlay

Dim _yawoffset As Long
Dim _rolloffset As Long
Dim _pitchoffset As Long

Dim _yawoffset_int As Integer
Dim _rolloffset_int As Integer
Dim _pitchoffset_int As Integer

Dim _yawnow As Integer
Dim _rollnow As Integer
Dim _pitchnow As Integer


Dim Acc_x As Byte
Dim Acc_y As Byte
Dim Acc_z As Byte

Dim Acc_x_now As Integer
Dim Acc_y_now As Integer
Dim Acc_z_now As Integer

Dim Acc_x_offset As Word
Dim Acc_y_offset As Word
Dim Acc_z_offset As Word

Dim Acc_x_offset_int As Integer
Dim Acc_y_offset_int As Integer
Dim Acc_z_offset_int As Integer

Dim Tmp As Byte
Dim I As Byte


Dim _yaw_kp_err As Integer
Dim _roll_kp_err As Integer
Dim _pitch_kp_err As Integer

Dim _yaw_ki_err As Integer
Dim _roll_ki_err As Integer
Dim _pitch_ki_err As Integer

Dim _yaw_ki_sum As Integer
Dim _roll_ki_sum As Integer
Dim _pitch_ki_sum As Integer

Dim _yaw_kd_err As Integer
Dim _roll_kd_err As Integer
Dim _pitch_kd_err As Integer

Dim _yaw_kd_old As Integer
Dim _roll_kd_old As Integer
Dim _pitch_kd_old As Integer

Dim _yaw_pid As Integer
Dim _roll_pid As Integer
Dim _pitch_pid As Integer

Dim _yaw_err As Integer
Dim _roll_err As Integer
Dim _pitch_err As Integer



Dim _sbl(4) As Integer
_sbl(1) = 0
_sbl(2) = 0
_sbl(3) = 0
_sbl(4) = 0


Const _yaw_kp = 1000
Const _roll_kp = 1000
Const _pitch_kp = 1000

Const _yaw_ki = 100
Const _roll_ki = 100
Const _pitch_ki = 100

Const _yaw_kd = 100
Const _roll_kd = 100
Const _pitch_kd = 100


Dim _yaw_filter As Integer
Dim _yaw_filter_2 As Integer
Dim _roll_filter As Integer
Dim _roll_filter_2 As Integer
Dim _pitch_filter As Integer
Dim _pitch_filter_2 As Integer


'DEBUG!!!
Dim Min_yaw As Integer
Dim Max_yaw As Integer
Dim Min_roll As Integer
Dim Max_roll As Integer
Dim Min_pitch As Integer
Dim Max_pitch As Integer
'########



Call Nunchuk_init()
Call Wmp_init()
Call Set_wmp_offset()
Call Set_nunchuk_offset()
Cls



Do


 Call Read_wmp_data()
 Call Nunchuk_read()

 _yawnow = Yaw - _yawoffset_int
 _rollnow = Roll - _rolloffset_int
 _pitchnow = Pitch - _pitchoffset_int

 Acc_x_now = Acc_x - Acc_x_offset_int
 Acc_y_now = Acc_y - Acc_y_offset_int
 Acc_z_now = Acc_z - Acc_z_offset_int

 Call Pid_regulator()

 If _yaw_pid < Min_yaw Then Min_yaw = _yaw_pid
 If _yaw_pid > Max_yaw Then Max_yaw = _yaw_pid
 If _roll_pid < Min_roll Then Min_roll = _roll_pid
 If _roll_pid > Max_roll Then Max_roll = _roll_pid
 If _pitch_pid < Min_pitch Then Min_pitch = _pitch_pid
 If _pitch_pid > Max_pitch Then Max_pitch = _pitch_pid

 Taste = Tastenabfrage()
 If Taste <> 0 Then
  If Taste = 1 Then Decr _direction
  If Taste = 2 Then Incr _direction
  If _direction > 3 Then _direction = 1
  If _direction < 1 Then _direction = 3
 End If

 Cls
 Locate 1 , 1
 Lcd _yaw_pid ; " : " ; _roll_pid ; " : " ; _pitch_pid ; "  "

 Locate 2 , 1
 If _direction = 1 Then
  Lcd "YAW: " ; Min_yaw ; "/" ; Max_yaw ; "   "
 Elseif _direction = 2 Then
  Lcd "ROLL: " ; Min_roll ; "/" ; Max_roll ; "   "
 Elseif _direction = 3 Then
  Lcd "PITCH: " ; Min_pitch ; "/" ; Max_pitch ; "   "
 End If


'(
 Cls
 Locate 1 , 1
 Lcd _yawnow
 Locate 1 , 6
 Lcd ": " ; _rollnow
 Locate 1 , 11
 Lcd ": " ; _pitchnow ; "  "
 Locate 2 , 1
 Lcd Acc_x_now
 Locate 2 , 6
 Lcd ": " ; Acc_y_now
 Locate 2 , 11
 Lcd ": " ; Acc_z_now
 Waitms 100
')

Loop


Sub Nunchuk_init()
    I2cstart
    I2cwbyte &HA4
    I2cwbyte &H40
    I2cwbyte &H00 .
    I2cstop
End Sub

Sub Nunchuk_read()
   I2cstart
   I2cwbyte &HA4
   I2cwbyte &H00
   I2cstop
   Waitms 1
   Nunchuk_buffer(1) = 0
   I2creceive &HA5 , Nunchuk_buffer(1) , 0 , 6
   Acc_x = Nunchuk_buffer(3) Eor &H17
   Acc_x = Acc_x + &H17
   Acc_y = Nunchuk_buffer(4) Eor &H17
   Acc_y = Acc_y + &H17
   Acc_z = Nunchuk_buffer(5) Eor &H17
   Acc_z = Acc_z + &H17
End Sub

Sub Set_nunchuk_offset()
   Acc_x_offset = 0
   Acc_y_offset = 0
   Acc_z_offset = 0
   For I = 1 To 100
      Call Nunchuk_read()
      Acc_x_offset = Acc_x_offset + Acc_x
      Acc_y_offset = Acc_y_offset + Acc_y
      Acc_z_offset = Acc_z_offset + Acc_z
   Next I
   Acc_x_offset = Acc_x_offset / 100
   Acc_y_offset = Acc_y_offset / 100
   Acc_z_offset = Acc_z_offset / 100
   Acc_x_offset_int = Acc_x_offset
   Acc_y_offset_int = Acc_y_offset
   Acc_z_offset_int = Acc_z_offset
End Sub


Sub Wmp_init()
   I2cstart
   I2cwbyte &HA6
   I2cwbyte &HFE
   I2cwbyte &H04
   I2cstop
End Sub

Sub Send_zero()
   I2cstart
   I2cwbyte &HA4
   I2cwbyte &H00
   I2cstop
End Sub

Sub Read_wmp_data()
   Gosub Send_zero
   I2creceive &HA4 , Wmplus_buffer(1) , 0 , 6
   Yaw1 = Wmplus_buffer(1)
   Roll1 = Wmplus_buffer(2)
   Pitch1 = Wmplus_buffer(3)
   Shift Wmplus_buffer(4) , Right , 2 : Yaw0 = Wmplus_buffer(4)
   Shift Wmplus_buffer(5) , Right , 2 : Roll0 = Wmplus_buffer(5)
   Shift Wmplus_buffer(6) , Right , 2 : Pitch0 = Wmplus_buffer(6)
   Shift Yaw , Right , 2
   Shift Roll , Right , 2
   Shift Pitch , Right , 2

   'IIR-Filter
   'filter_wert = filter_wert * s + wert * (1.0-s)

    _yaw_filter = _yaw_filter * 4
    _yaw_filter = _yaw_filter / 10
    _yaw_filter_2 = Yaw * 6
    _yaw_filter_2 = _yaw_filter_2 / 10
    _yaw_filter = _yaw_filter + _yaw_filter_2
    Yaw = _yaw_filter

    _roll_filter = _roll_filter * 4
    _roll_filter = _roll_filter / 10
    _roll_filter_2 = Roll * 6
    _roll_filter_2 = _roll_filter_2 / 10
    _roll_filter = _roll_filter + _roll_filter_2
    Roll = _roll_filter

    _pitch_filter = _pitch_filter * 4
    _pitch_filter = _pitch_filter / 10
    _pitch_filter_2 = Pitch * 6
    _pitch_filter_2 = _pitch_filter_2 / 10
    _pitch_filter = _pitch_filter + _pitch_filter_2
    Pitch = _pitch_filter

End Sub

Sub Set_wmp_offset()
   _yawoffset = 0
   _rolloffset = 0
   _pitchoffset = 0
   For I = 1 To 100
      Call Read_wmp_data()
      _yawoffset = _yawoffset + Yaw
      _rolloffset = _rolloffset + Roll
      _pitchoffset = _pitchoffset + Pitch
   Next I
   _yawoffset = _yawoffset / 100
   _rolloffset = _rolloffset / 100
   _pitchoffset = _pitchoffset / 100
   _yawoffset_int = _yawoffset + 10
   _rolloffset_int = _rolloffset + 10
   _pitchoffset_int = _pitchoffset + 10
End Sub

Sub Pid_regulator()
 _yaw_err = _sbl(4) / 1
 _yaw_err = _yaw_err - _yawnow

 _roll_err = _sbl(3) / 5
 _roll_err = _roll_err - _rollnow

 _pitch_err = _sbl(2) / 5
 _pitch_err = _pitch_err - _pitchnow


 _yaw_kp_err = _yaw_kp_err * _yaw_kp
 _yaw_kp_err = _yaw_err / 10

 _roll_kp_err = _roll_kp_err * _roll_kp
 _roll_kp_err = _roll_err / 10

 _pitch_kp_err = _pitch_kp_err * _pitch_kp
 _pitch_kp_err = _pitch_err / 10


 _yaw_ki_err = _yaw_ki_err * _yaw_ki
 _yaw_ki_err = _yaw_err / 50
 _yaw_ki_sum = _yaw_ki_sum + _yaw_ki_err

 _roll_ki_err = _roll_ki_err * _roll_ki
 _roll_ki_err = _roll_err / 50
 _roll_ki_sum = _roll_ki_sum + _roll_ki_err

 _pitch_ki_err = _pitch_ki_err * _pitch_ki
 _pitch_ki_err = _pitch_err / 50
 _pitch_ki_sum = _pitch_ki_sum + _pitch_ki_err


 _yaw_kd_err = _yaw_kd_err * _yaw_kd
 _yaw_kd_err = _yaw_err / 50
 _yaw_kd_err = _yaw_kd_old - _yaw_kd_err
 _yaw_kd_old = _yaw_kd_old

 _roll_kd_err = _roll_kd_err * _roll_kd
 _roll_kd_err = _roll_err / 50
 _roll_kd_err = _roll_kd_old - _roll_kd_err
 _roll_kd_old = _roll_kd_old

 _pitch_kd_err = _pitch_kd_err * _pitch_kd
 _pitch_kd_err = _pitch_err / 50
 _pitch_kd_err = _pitch_kd_old - _pitch_kd_err
 _pitch_kd_old = _pitch_kd_old


 _yaw_pid = _yaw_kp_err + _yaw_ki_sum
 _yaw_pid = _yaw_pid + _yaw_kd_err

 _roll_pid = _roll_kp_err + _roll_ki_sum
 _roll_pid = _roll_pid + _roll_kd_err

 _pitch_pid = _pitch_kp_err + _pitch_ki_sum
 _pitch_pid = _pitch_pid + _pitch_kd_err


 If _yaw_pid < -1000 Then _yaw_pid = -1000
 If _yaw_pid > 1000 Then _yaw_pid = 1000
 If _roll_pid < -1000 Then _roll_pid = -1000
 If _roll_pid > 1000 Then _roll_pid = 1000
 If _pitch_pid < -1000 Then _pitch_pid = -1000
 If _pitch_pid > 1000 Then _pitch_pid = 1000
End Sub

Function Tastenabfrage() As Byte
Local Ws As Word
   Tastenabfrage = 0
   Start Adc
   Ws = Getadc(7)
   If Ws < 500 Then
      Select Case Ws
         Case 400 To 450
            Tastenabfrage = 1
         Case 330 To 380
            Tastenabfrage = 2
         Case 260 To 305
            Tastenabfrage = 3
         Case 180 To 220
            Tastenabfrage = 4
         Case 90 To 130
            Tastenabfrage = 5
      End Select
   End If
   Waitms 100
End Function

End

Wenn jemand ein Video vom Display sehen möchte, kann ichs schnell filmen! Die WM+ liegt seit ca. 30 Min neben mir und es gab noch keinen einzigen (!!!) "Ausbrecher". Ich denke, das genügt vollkommen zum fliegen?!
ABER:
Durch meine Division durch 4 verliere ich an Auflösung, d.h. ich muss auch die Werte für kp, ki, kd extrem hochschrauben... Denkt ihr, das ist ok oder würdet ihr das anders lösen?

Viele Fragen auf einmal, ich hoffe auf zahlreiche Antworten
Gruß
Chris

[lokirobotics]

Guck dir mal das hier an. Driftkompensation durch Zusammenarbeit von Gyro und Beschleunigungssensor mit Kalman Filter.

[Che Guevara]

Danke für den Tip, die Seite scheint ganz gut zu sein Leider weiß ich nicht, mit welchem Programm ich die Files (und va inwelchem ist der kalman filter) öffnen kann? Geht das mit AVRStudio? Außerdem verstehe ich die C-Syntax nicht... Aber dennoch möchte ichs mir ansehen, wenn mir jemand das richtige Programm nennt.

Gruß
Chris

[lokirobotics]

Der Kalman Filter ist in den Dateien ars.h und ars.c implementiert.
In diesem Artikel werden die Grundlagen zum Filter erklärt.

[Che Guevara]

Also ich hab mir jetzt die 2 Datein angesehn (mit AVRStudio) und muss sagen, ich verstehe leider fast garnichts Was bedeutet z.b. der "->" (Pfeil) bei

filterdata->xx = ...

Gibts nicht evtl. einen anderen Code, den ich als BASCOM-Jünger besser verstehe? Ich weiß, dass hier im Netz auch ein Kalman-Filter in Bascom geschrieben wurde, diesen würde ich zumindest soweit verstehen, dass ich Änderungen vornehmen könnte! Leider weiß ich aber nicht, wie ich da den ACC mit einrechnen soll, also mittels welcher Variable?

Gruß
Chris

[lokirobotics]

filterdata ist eine Variable der Datenstruktur ...1DKalmanFilter... "->" ist der Zugriffsoperator, mit dem auf die Felder in der Datenstruktur zugegriffen werden kann.

[Che Guevara]

Hm, ich glaube, der Kalman-Filter ist mir zu kompliziert! Aber Willa betont ja auch immer, ein Komplementärfilter ist genauso gut wie ein Kalman Gibts da evtl. ein paar mehr Beispiele für so einen Komplementärfilter? Bis jetzt habe ich nur 1 Beispiel im web gefunden, welches jedoch für mich sehr unverständlich ist, da nicht viel kommentiert wurde... Hat da jemand was?

Gruß
Chris