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

**Che Guevara** · 22.05.2011, 10:55

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:

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.

**Che Guevara** · 23.05.2011, 18:54

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:

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?

**lokirobotics** · 26.05.2011, 14:48

Du müsstest mal genau erklären, was du eigentlich filtern möchtest.
Die von dir angesprochenen Filter dienen ja dazu Signale bestimmter Frequenzen passieren zu lassen, oder sie halt zu blocken.
Den Drift deiner Gyros wirst du mit einem Frequenzfilter aber nicht kompensiert bekommen.
Wenn das Gyro "driftet" heißt das ja, dass es eine Drehrate ausgibt, die von der tatsächlichen Drehrate abweicht.
Beispielsweise gibt das Gyro immer eine Drehrate aus, die um den Faktor 0,1 größer ist, als die Tatsächliche.
Wenn du über diesen Wert integrierst, wird die Abweichung von deiner Systemdarstellung zum Systemzustand mit der Zeit immer größer.

Du musst den Drift also Kompensieren, nicht filtern.
Ich würde dazu ersteinmal das Driftverhalten der Gyros analysieren (stetiger Drift, variabler Drift) und darauf dann entsprechende Gegenmaßnahmen einleiten.

**Che Guevara** · 26.05.2011, 16:40

Hallo,

also einerseits möchte ich das RC-Empfänger Signal filtern, d.h. Störungen rausholen. Diese Störungen sind ja eher hochfrequenter Art, also brauche ich einen Tiefpass (IIR-Filter).. Sehe ich das so richtig?
Dann möchte ich noch den Drift vom Gyro kompensieren, aber ich weiß eben nicht, wie ich das machen soll!? ... Das mit dem Bestimmen des Drifts werde ich demnächst mal machen! Aber was dann, wenn ich weiß, dass der Drift z.b. variabel ist? Dann ist meine einzige Rettung ein ACC oder?

Gruß
Chris

**lokirobotics** · 27.05.2011, 14:13

Zu den Störungen kann ich nichts sagen, da ich den Sensor nicht kenne. Klingt aber erstmal plausibel.

Variabler Drift wäre natürlich ganz schlecht. Wenn er aber konstant ist, oder sich als Funktion der Zeit und evtl. der Temperatur darstellen lässt, müsste man ganz gut kompensieren können.
Ggf. könnte man auch die Änderung der Drehrate als "Frequenz" auffassen und die Änderungen durch einen Hochpass schicken.

Ich hab mich noch nicht mit Driftkompensation auseinandergesetzt. Das sind jetzt so die Sachen, die mir dazu einfallen.

Lg

**PICture** · 27.05.2011, 14:17

Hallo!

Eigentlich als zufälliges Signall, lässt sich Drift praktisch nicht kompensieren, höchstens minimieren.

**Che Guevara** · 27.05.2011, 15:19

Hallo,

das mit der Nichtkompensierbarkeit *gg* von dem variablen Drift habe ich mir schon fast gedacht, deswegen auch meine Frage...
Heute hab ich mir mal einen NUNCHUK zugelegt; dieser ist bereits zerlegt und demnächst werde ich mal eine Platine ätzen, auf der sich der Nunchuk und die WM+ befinden... Diese wird dann testweise am PC angesteckt (über ein RN-Control), um zu sehen, wie ich die Werte am besten interpretiere / filtere / etc...
Wenn ich neue Infos habe, melde ich mich mal wieder, wird so gegen 9 / 10 heute Abend sein (wenn ichs noch schaffe).

Gruß
Chris

**Che Guevara** · 28.05.2011, 03:34

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:

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** · 31.05.2011, 11:10

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

**Che Guevara** · 31.05.2011, 13:59

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