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

[Che Guevara]

Hallo,

ich habe jetzt mal einen IIR-Filter für den RC-Empfänger implementiert, das hat schon eine kleine Verbesserung gebracht! Jedoch Suche ich immer noch händeringend nach einem Filter für die Gyro-Daten... Soll ichs dort auch mal mit einem IIR probieren?
Hier noch der Code:

$regfile = "m8def.dat"
$crystal = 16000000
$framesize = 140
$hwstack = 120
$swstack = 120
$baud = 38400


Declare Sub Wmp_init()
Declare Sub Send_zero()
Declare Sub Read_data()
Declare Sub Set_offset()

Declare Sub Filter_gyro_data()
Declare Sub Pid_regulator()
Declare Sub Filter_rx_data()
Declare Sub Mixer()
Declare Sub Send_data()


$lib "I2C_TWI.LBX"                                          'Hardware I2C
Config Scl = Portc.5                                        'Ports for I2C-Bus
Config Sda = Portc.4
Config Twi = 100000
                     '400000
I2cinit


Config Timer1 = Timer , Prescale = 1
On Timer1 Pausedetect
Enable Timer1

Config Int1 = Falling
On Int1 Measure
Enable Int1


Config Pind.3 = Input
Portd.3 = 0


Const Start_byte = 127
Const _maxchannel = 4

Const _bl1offset = 0
Const _bl2offset = 0
Const _bl3offset = 0
Const _bl4offset = -600

Const _yaw_kp = 5
Const _roll_kp = 5
Const _pitch_kp = 5

Const _yaw_ki = 3
Const _roll_ki = 3
Const _pitch_ki = 3

Const _yaw_kd = 5
Const _roll_kd = 5
Const _pitch_kd = 5


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_filter(4) As Integer
Dim _sbl_filter_2(4) As Integer


Dim Bufferbyte As Byte
Dim Kanal(_maxchannel) As Word
Dim Channel As Byte
Dim _bl(_maxchannel) As Word
Dim I As Byte
Dim _crc As Word
Dim _sbl(_maxchannel) As Integer

Dim 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 _yawnow As Integer
Dim _rollnow As Integer
Dim _pitchnow As Integer

Dim _empfmiddle(_maxchannel) As Word
Dim _empfmin(_maxchannel) As Word
Dim _empfmax(_maxchannel) As Word
Dim _empfdiv(_maxchannel) As Word
Dim _stick_sensitivy(_maxchannel) As Word

_stick_sensitivy(1) = 2265
_stick_sensitivy(2) = 1800
_stick_sensitivy(3) = 1800
_stick_sensitivy(4) = 2265

_empfmiddle(1) = 26500
_empfmiddle(2) = 23800
_empfmiddle(3) = 25300
_empfmiddle(4) = 22250

_empfmin(1) = 14300
_empfmin(2) = 14650
_empfmin(3) = 17100
_empfmin(4) = 14750

_empfmax(1) = 32300
_empfmax(2) = 32600
_empfmax(3) = 32600
_empfmax(4) = 30500



For I = 1 To 4
 _empfdiv(i) = _empfmiddle(i) - _empfmin(i)
 _empfdiv(i) = _empfdiv(i) / _stick_sensitivy(i)            '2265
 _empfdiv(i) = _empfdiv(i) * 2
Next I


Call Wmp_init()
Waitms 500
Call Set_offset()
Enable Interrupts



Do


 Call Filter_rx_data()
 Call Filter_gyro_data()
 Call Pid_regulator()
 Call Mixer()
 Call Send_data()


Loop


Measure:
If Channel > 0 And Channel < 5 Then
  Kanal(channel) = Timer1
End If
Timer1 = 1536
Incr Channel
Return

Pausedetect:
Channel = 0
Return


Sub Wmp_init()
    I2cstart
    I2cwbyte &HA6                                           ' sends memory address
    I2cwbyte &HFE                                           ' WM+ activation
    I2cwbyte &H04 .                                         ' Now Adress changes to &HA4
    I2cstop
End Sub

Sub Send_zero()
   I2cstart
   I2cwbyte &HA4                                            ' sends memory address
   I2cwbyte &H00                                            ' sends zero before receiving
   I2cstop
   Waitms 1
End Sub

Sub Read_data()
   Gosub Send_zero                                          ' sends zero before receiving
   I2creceive &HA4 , Buffer(1) , 0 , 6                      ' receive 6 bytes
   Yaw1 = Buffer(1)
   Roll1 = Buffer(2)                                        '  Low Bytes
   Pitch1 = Buffer(3)
   Shift Buffer(4) , Right , 2 : Yaw0 = Buffer(4)
   Shift Buffer(5) , Right , 2 : Roll0 = Buffer(5)          '  High Bytes
   Shift Buffer(6) , Right , 2 : Pitch0 = Buffer(6)
End Sub

Sub Set_offset()
   _yawoffset = 0
   _rolloffset = 0
   _pitchoffset = 0
   For I = 1 To 100
      Call Read_data
      _yawoffset = _yawoffset + Yaw
      _rolloffset = _rolloffset + Roll
      _pitchoffset = _pitchoffset + Pitch
   Next I
   _yawoffset = _yawoffset / 100
   _rolloffset = _rolloffset / 100
   _pitchoffset = _pitchoffset / 100
End Sub



Sub Filter_gyro_data()
 Call Read_data()
 _yawnow = Yaw - _yawoffset
 _rollnow = Roll - _rolloffset
 _pitchnow = Pitch - _pitchoffset

 _yawnow = _yawnow / 2
 _rollnow = _rollnow / 2
 _pitchnow = _pitchnow / 2

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


Sub Filter_rx_data()

 For I = 1 To 4
  _sbl(i) = Kanal(i) - _empfmiddle(i)
  _sbl(i) = _sbl(i) / _empfdiv(i)
 Next I

 '_sbl(4) = _sbl(4) * -1

 _sbl(2) = _sbl(2) / 2
 _sbl(3) = _sbl(3) / 2
 '_sbl(4) = _sbl(4) / 2

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

 For I = 1 To 4
  _sbl_filter(i) = _sbl_filter(i) * 6
  _sbl_filter(i) = _sbl_filter(i) / 10
  _sbl_filter_2(i) = _sbl(i) * 4
  _sbl_filter_2(i) = _sbl_filter_2(i) / 10
  _sbl_filter(i) = _sbl_filter(i) + _sbl_filter_2(i)
  _sbl(i) = _sbl_filter(i)
 Next I

End Sub


Sub Mixer()
 _bl(1) = 62667 - _sbl(1)
 _bl(2) = _bl(1)
 _bl(3) = _bl(1)
 _bl(4) = 62667

 _bl(1) = _bl(1) + _bl1offset
 _bl(2) = _bl(2) + _bl2offset
 _bl(3) = _bl(3) + _bl3offset
 _bl(4) = _bl(4) + _bl4offset

 _bl(1) = _bl(1) + _pitch_pid
 _bl(2) = _bl(2) + _roll_pid
 _bl(3) = _bl(3) - _roll_pid
 _bl(4) = _bl(4) - _yaw_pid
End Sub


Sub Send_data()
 If Channel <= 11 Then
  _crc = Crc16(_bl(1) , 4)
  Printbin Start_byte ; _bl(1) ; _bl(2) ; _bl(3) ; _bl(4) ; _crc
 End If
End Sub


End

Gruß
Chris

EDIT:
Weiß das keiner? Evtl. sollte ich meine Frage nochmal umformulieren:
Welchen Filter würdet ihr persönlich nehmen, um die Gyro-Daten vom Wii Motion Plus Gyro zu filtern?