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

[Che Guevara]

Hallo,

nachdem ich nun erfolgreich meinen PID-Regler implementiert habe, möchte ich nun die Gyro-Signale meiner WM+ filtern, um etwas bessere Werte zu erhalten. Aber ich weiß leider nicht so genau, wie ich die Signale filtern soll?!
1. Mit einem Komplementärfilter (also Hochpass und Tiefpassfilter, oder?!)
2. Mit einem Kalmanfilter (ist mir ehrlich gesagt zu kompliziert)
3. Nur mit einem Hochpass bzw. nur mit einem Tiefpassfilter

Was wäre eurer Meinung nach am besten geeignet? Der Tricopter soll jetzt nicht da minutelang in der Luft schweben, aber momentan ist es leider so, dass die Motoren vereinzelt schon im Stand anfangen, sich zu drehen, wegen dem Drift...
Außerdem möchte ich noch einen Filter für den RC-Empfänger implementieren, da diese Werte auch geringfügig schwanken. Welchen Filter sollte ich den hier verwenden?
Hier noch der jetztige 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 = 4
Const _roll_kp = 4
Const _pitch_kp = 4

Const _yaw_ki = 2
Const _roll_ki = 2
Const _pitch_ki = 2

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 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) = 1800

_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()

Wait 2

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
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

Vielen Dank
Gruß
Chris

EDIT:
Wenn jemand einen Beispielcode oder eine Seite hätte, auf der man ein Beispiel einer Implementierung sieht, wäre ich nicht abgeneigt, da sich das Internet über solche Implementaitionen doch sehr ausschweigt :/
Ein Tiefpass-Filter ist mir eigtl. klar, das ist im Prinzip nichts anderes als eine Durchschnittsbildung.