Hallo,

mein Tricopter ist nun soweit fertig, dass ich ihn denke ich auch vorzeigen kann Allerdings gibt es noch ein paar Sachen, die mich stören! Das erste Problem ist folgendes:
Wenn ich in eine Richtung steuere, fliegt der Tri auch dorthin, allerdings dauert es ziemlich lange, bis er sich wieder in die Waagrechte Position begibt, nachdem ich den Knüppel loslasse (z.b. roll). Eigentlich sollte man das ja mit dem D-Anteil des PID-Reglers schneller machen können, aber mein D-Anteil steht momentan auf 0, da der Tri ansonsten zu Schwingen beginnt. Wie kann ich es nun erreichen, dass er schneller wieder die "Ruheposition" einnimmt? Den I-Anteil vergrößern klappt nicht, da er sonst auch schwingt! Hier ist auch das nächste Problem: Die Dimensionierung des Anti-Windup für den I-Anteil. Ich habe momentan keinerlei Anhaltspunkte über desen Größenordnung und ehrlich gesagt kommt Try and Error für mich in diesem Fall nicht in Frage, da ich das schon einmal gemacht habe und dabei ist mir der halbe Tri auseinandergebrochen, weil der max. Wert des I-Anteils zu klein war.... Aber um das geht es hier jetzt nicht (momentan).

Wäre es möglich, die Stickposition (also die Werte der Kanäle) mit dem letzten Wert zu differenzieren und somit dann gegenzusteuern? Ich denke, das sollte funktionieren, aber bevor ich irgendetwas schrotte, dachte ich, ich frage mal hier nach?!

Code:
$regfile = "m328pdef.dat"
$crystal = 16000000
$framesize = 200
$hwstack = 200
$swstack = 200
$baud = 1000000                                             '115200


Open "COMC.3:19200,8,N,1" For Input As #1
Open "COMC.2:19200,8,N,1" For Output As #2


Declare Sub Init_system()

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

Declare Sub Pid_regulator()

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



$lib "I2C_TWI.LBX"
Config Scl = Portc.5
Config Sda = Portc.4
Config Twi = 100000
I2cinit


Config Timer0 = Timer , Prescale = 256
On Timer0 Pausedetect
Enable Timer0

Config Int0 = Falling
On Int0 Measure
Enable Int0


Config Pind.2 = Input
Portd.2 = 0


Config Portd.4 = Output
Led Alias Portd.4
Led = 0


'####################################
'###########KONSTANTEN###############
'####################################
Const _maxchannel = 8
Const Start_byte = 127

Const _throttlechannel = 1
Const _rollchannel = 2
Const _pitchchannel = 3
Const _yawchannel = 4
Const _statechannel = 5
Const _datachannel = 6

Const _bl4offset = 0

Const _max_d_state = 3
'###################################
'###################################
'###################################


'###################################
'###########PID-PARAMETER###########
'###################################
Dim _yaw_kp As Single
Dim _roll_kp As Single
Dim _pitch_kp As Single

Dim _yaw_ki As Single
Dim _roll_ki As Single
Dim _pitch_ki As Single

Dim _yaw_kd As Single
Dim _roll_kd As Single
Dim _pitch_kd As Single

'#########PID-Werte############
'(
P : 0.675000008:1.049999949
I : 0.00139999:0.004399952
D:
')
'##############################
_yaw_kp = 1.5
_roll_kp = 0.675000008
_pitch_kp = 1.049999949

_yaw_ki = 0
_roll_ki = 0.00139999
_pitch_ki = 0.004399952

_yaw_kd = 0
_roll_kd = 0
_pitch_kd = 0.00079995
'###################################
'###################################
'###################################


'###################################
'###############TMP#################
'###################################
Dim _btm222_counter As Byte
Dim I As Byte
Dim Newvalsreceived As Bit
'###################################
'###################################
'###################################



'###################################
'#########RC-EMPFÄNGER##############
'###################################
Dim Bufferbyte As Byte
Dim Kanal(_maxchannel) As Word
Dim _lp_bandwidth As Byte
Dim _hkanal(_maxchannel) As Word
Dim _lkanal(_maxchannel) As Word
Dim _kanal_filter(_maxchannel) As Word
Dim Channel As Byte
Dim _bl(4) As Word
Dim _crc As Word
Dim _sbl(_maxchannel) As Integer
Dim _sbl_filter(_maxchannel) As Integer
'###################################
'###################################
'###################################


'###################################
'###########I2C-Inputs##############
'###################################
Dim Wmplus_buffer(6) As Byte
'###################################
'###################################
'###################################


'###################################
'#########GYRO######################
'###################################
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
'###################################
'###################################
'###################################


'##################################
'#########PID-REGLER###############
'##################################
Dim _d_state As Byte

Dim _yaw_kp_err As Single
Dim _roll_kp_err As Single
Dim _pitch_kp_err As Single

Dim _yaw_ki_err As Single
Dim _roll_ki_err As Single
Dim _pitch_ki_err As Single

Dim _yaw_ki_sum As Single
Dim _roll_ki_sum As Single
Dim _pitch_ki_sum As Single

Dim _yaw_kd_err_single As Single
Dim _roll_kd_err_single As Single
Dim _pitch_kd_err_single As Single

Dim _yaw_kd_err(_max_d_state) As Single
Dim _roll_kd_err(_max_d_state) As Single
Dim _pitch_kd_err(_max_d_state) As Single

Dim _yaw_kd_old(_max_d_state) As Single
Dim _roll_kd_old(_max_d_state) As Single
Dim _pitch_kd_old(_max_d_state) As Single

Dim _yaw_pid_int As Integer
Dim _roll_pid_int As Integer
Dim _pitch_pid_int As Integer

Dim _yaw_pid As Single
Dim _roll_pid As Single
Dim _pitch_pid As Single

Dim _yaw_err_int As Integer
Dim _roll_err_int As Integer
Dim _pitch_err_int As Integer

Dim _yaw_err As Single
Dim _roll_err As Single
Dim _pitch_err As Single
'#################################
'#################################
'#################################

Dim _yawstickvel As Integer
Dim _rollstickvel As Integer
Dim _pitchstickvel As Integer

Dim _sbl_old(_maxchannel) As Integer


Dim _x1 As Single
Dim _x2 As Single


Wait 2

Call Init_system()

Enable Interrupts




Do


 Call Filter_rx_data()                                      '580
 Call Filter_gyro_data()                                    '1900
 Call Pid_regulator()                                       '700
 Call Mixer()                                               '210
 Call Send_data()                                           '470


Loop


Sub Filter_gyro_data()
 Call Read_wmp_data()

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


Sub Filter_rx_data()

 If Newvalsreceived = 1 Then
   Newvalsreceived = 0

   If Kanal(_throttlechannel) >= 60 And Kanal(_throttlechannel) <= 140 Then
     _sbl(_throttlechannel) = Kanal(_throttlechannel) - 100
     _sbl(_throttlechannel) = _sbl(_throttlechannel) * 30
     If _sbl(_throttlechannel) > 900 Then _sbl(_throttlechannel) = 900
     If _sbl(_throttlechannel) < -1000 Then _sbl(_throttlechannel) = -1000
   End If

   For I = 2 To _maxchannel
     If Kanal(i) >= 60 And Kanal(i) <= 140 Then
       _sbl(i) = Kanal(i) - 100
       If _sbl(i) > -2 And _sbl(i) < 2 Then _sbl(i) = 0
       _sbl(i) = _sbl(i) * 25
     End If
   Next I

   _sbl_filter(_throttlechannel) = _sbl_filter(_throttlechannel) + _sbl(_throttlechannel)
   _sbl_filter(_throttlechannel) = _sbl_filter(_throttlechannel) / 2
   _sbl(_throttlechannel) = _sbl_filter(_throttlechannel)

   _sbl_filter(_rollchannel) = _sbl_filter(_rollchannel) + _sbl(_rollchannel)
   _sbl_filter(_rollchannel) = _sbl_filter(_rollchannel) / 2
   _sbl(_rollchannel) = _sbl_filter(_rollchannel)

   _sbl_filter(_pitchchannel) = _sbl_filter(_pitchchannel) + _sbl(_pitchchannel)
   _sbl_filter(_pitchchannel) = _sbl_filter(_pitchchannel) / 2
   _sbl(_pitchchannel) = _sbl_filter(_pitchchannel)

   _sbl_filter(_yawchannel) = _sbl_filter(_yawchannel) + _sbl(_yawchannel)
   _sbl_filter(_yawchannel) = _sbl_filter(_yawchannel) / 2
   _sbl(_yawchannel) = _sbl_filter(_yawchannel)

   _sbl_filter(_statechannel) = _sbl_filter(_statechannel) * 19
   _sbl_filter(_statechannel) = _sbl_filter(_statechannel) + _sbl(_statechannel)
   _sbl_filter(_statechannel) = _sbl_filter(_statechannel) / 20
   _sbl(_statechannel) = _sbl_filter(_statechannel)

   _sbl_filter(_datachannel) = _sbl_filter(_datachannel) + _sbl(_datachannel)
   _sbl_filter(_datachannel) = _sbl_filter(_datachannel) / 2
   _sbl(_datachannel) = _sbl_filter(_datachannel)


   If _sbl(_statechannel) < 200 Then
     _sbl(_rollchannel) = _sbl(_rollchannel) / 5            '5
     _sbl(_pitchchannel) = _sbl(_pitchchannel) / 5
   Elseif _sbl(_statechannel) > 200 Then
     _sbl(_rollchannel) = _sbl(_rollchannel) / 2            '3
     _sbl(_pitchchannel) = _sbl(_pitchchannel) / 3
   End If

   _sbl(_yawchannel) = _sbl(_yawchannel) / 2


   _yawstickvel = _sbl(_yawchannel) - _sbl_old(_yawchannel)
   _sbl_old(_yawchannel) = _sbl(_yawchannel)

   _rollstickvel = _sbl(_rollchannel) - _sbl_old(_rollchannel)
   _sbl_old(_rollchannel) = _sbl(_rollchannel)

   _pitchstickvel = _sbl(_pitchchannel) - _sbl_old(_pitchchannel)
   _sbl_old(_pitchchannel) = _sbl(_pitchchannel)

'(
   _kanal_filter(7) = _kanal_filter(7) * 3
   _kanal_filter(7) = _kanal_filter(7) + Kanal(7)
   Shift _kanal_filter(7) , Right , 2
   Kanal(7) = _kanal_filter(7)

   _kanal_filter(8) = _kanal_filter(8) * 3
   _kanal_filter(8) = _kanal_filter(8) + Kanal(8)
   Shift _kanal_filter(8) , Right , 2
   Kanal(8) = _kanal_filter(8)
')
'(
'#########P-GAIN###########
   _x1 = Kanal(7) - 100
   _x1 = _x1 * 0.075
   _x2 = Kanal(8) - 100
   _x2 = _x2 * 0.075

   _roll_kp = 0 + _x1
   _pitch_kp = 0 + _x2

'#########I-GAIN###########
   _x1 = Kanal(7) - 100
   _x1 = _x1 * 0.0002
   _x2 = Kanal(8) - 100
   _x2 = _x2 * 0.0002

   _roll_ki = 0.0 + _x1
   _pitch_ki = 0.0 + _x2

'#########D-GAIN###########
   _x1 = Kanal(7) - 100
   _x1 = _x1 * 0.0002
   _x2 = Kanal(8) - 100
   _x2 = _x2 * 0.0002

   _roll_kd = 0.0 + _x1
   _pitch_kd = 0.0 + _x2
')


   If _sbl(_pitchchannel) > 170 And _sbl(_yawchannel) > 200 And _sbl(_throttlechannel) < -950 And _sbl(_statechannel) < -200 Then
     Call Set_wmp_offset()
     Led = 0
     If _sbl(_datachannel) > -200 Then
       Print #2 , "OFFSET neu berechnet!"
     End If
   End If

 End If

End Sub


Measure:
 If Channel > 0 And Channel < 9 Then
   Kanal(channel) = Timer0
 End If
 Timer0 = 6
 Incr Channel
Return


Pausedetect:
  If Channel <> 0 Then
    Newvalsreceived = 1
  End If
  Channel = 0
Return


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


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


Sub Pid_regulator()

'##############FEHLER_BERECHNEN##########
'(
 _yaw_err_int = _sbl(_yawchannel) + _yawstickvel
 _yaw_err_int = _yaw_err_int - _yawnow
 _yaw_err = _yaw_err_int

 _roll_err_int = _sbl(_rollchannel) + _rollstickvel
 _roll_err_int = _roll_err_int - _rollnow
 _roll_err = _roll_err_int

 _pitch_err_int = _sbl(_pitchchannel) + _pitchstickvel
 _pitch_err_int = _pitch_err_int - _rollnow
 _pitch_err = _pitch_err_int
')

 _yaw_err_int = _sbl(_yawchannel) - _yawnow
 _yaw_err = _yaw_err_int

 _roll_err_int = _sbl(_rollchannel) - _rollnow
 _roll_err = _roll_err_int

 _pitch_err_int = _sbl(_pitchchannel) - _pitchnow
 _pitch_err = _pitch_err_int

'##############PROPORTIONAL##############

 _yaw_kp_err = _yaw_err * _yaw_kp

 _roll_kp_err = _roll_err * _roll_kp

 _pitch_kp_err = _pitch_err * _pitch_kp

'##############INTEGRAL##################

 _yaw_ki_err = _yaw_err * _yaw_ki
 _yaw_ki_sum = _yaw_ki_sum + _yaw_ki_err

 _roll_ki_err = _roll_err * _roll_ki
 _roll_ki_sum = _roll_ki_sum + _roll_ki_err

 _pitch_ki_err = _pitch_err * _pitch_ki
 _pitch_ki_sum = _pitch_ki_sum + _pitch_ki_err

'##############DIFFERENTIAL#############

 Incr _d_state
 If _d_state > _max_d_state Then _d_state = 1

 _yaw_kd_err(_d_state) = _yaw_err * _yaw_kd
 _yaw_kd_err(_d_state) = _yaw_kd_err(_d_state) - _yaw_kd_old(_d_state)       'evtl. anders herum!!!!!!!!!!
 _yaw_kd_old(_d_state) = _yaw_kd_err(_d_state)

 _roll_kd_err(_d_state) = _roll_err * _roll_kd
 _roll_kd_err(_d_state) = _roll_kd_err(_d_state) - _roll_kd_old(_d_state)       'evtl. anders herum!!!!!!!!!!
 _roll_kd_old(_d_state) = _roll_kd_err(_d_state)

 _pitch_kd_err(_d_state) = _pitch_err * _pitch_kd
 _pitch_kd_err(_d_state) = _pitch_kd_err(_d_state) - _pitch_kd_old(_d_state)       'evtl. anders herum!!!!!!!!!!
 _pitch_kd_old(_d_state) = _pitch_kd_err(_d_state)

 _yaw_kd_err_single = _yaw_kd_err(_d_state)
 _roll_kd_err_single = _roll_kd_err(_d_state)
 _pitch_kd_err_single = _pitch_kd_err(_d_state)

'##############AUFSUMMIEREN##############

 _yaw_pid = _yaw_kp_err + _yaw_ki_sum
 _yaw_pid = _yaw_pid + _yaw_kd_err_single
 _yaw_pid_int = _yaw_pid

 _roll_pid = _roll_kp_err + _roll_ki_sum
 _roll_pid = _roll_pid + _roll_kd_err_single
 _roll_pid_int = _roll_pid

 _pitch_pid = _pitch_kp_err + _pitch_ki_sum
 _pitch_pid = _pitch_pid + _pitch_kd_err_single
 _pitch_pid_int = _pitch_pid

'###############WERTE_BEGRENZEN##########

 If _yaw_pid_int < -1000 Then _yaw_pid_int = -1000
 If _yaw_pid_int > 1000 Then _yaw_pid_int = 1000
 If _roll_pid_int < -1000 Then _roll_pid_int = -1000
 If _roll_pid_int > 1000 Then _roll_pid_int = 1000
 If _pitch_pid_int < -1000 Then _pitch_pid_int = -1000
 If _pitch_pid_int > 1000 Then _pitch_pid_int = 1000

End Sub


Sub Mixer()

 _bl(1) = 62667 - _sbl(_throttlechannel)
 _bl(2) = _bl(1)
 _bl(3) = _bl(1)
 _bl(4) = 62667

 _bl(4) = _bl(4) + _bl4offset

 If _sbl(_statechannel) > -200 Then
   _bl(1) = _bl(1) + _pitch_pid_int
   _bl(2) = _bl(2) - _roll_pid_int
   _bl(3) = _bl(3) + _roll_pid_int
   _pitch_pid_int = _pitch_pid_int / 2
   _bl(2) = _bl(2) - _pitch_pid_int
   _bl(3) = _bl(3) - _pitch_pid_int
   _bl(4) = _bl(4) - _yaw_pid_int
   Led = 1
 Elseif _sbl(_statechannel) < -200 Then
   _bl(1) = 63800
   _bl(2) = 63800
   _bl(3) = 63800
   For I = 0 To _max_d_state
     _yaw_kd_err(i) = 0
     _yaw_kd_old(i) = 0
     _roll_kd_err(i) = 0
     _roll_kd_old(i) = 0
     _pitch_kd_err(i) = 0
     _pitch_kd_old(i) = 0
   Next I
   _yaw_kp_err = 0
   _yaw_ki_sum = 0
   _yaw_kd_err_single = 0
   _yaw_ki_err = 0
   _roll_kp_err = 0
   _roll_ki_sum = 0
   _roll_kd_err_single = 0
   _roll_ki_err = 0
   _pitch_kp_err = 0
   _pitch_ki_sum = 0
   _pitch_kd_err_single = 0
   _pitch_ki_err = 0
   _yaw_pid_int = 0
   _roll_pid_int = 0
   _pitch_pid_int = 0
   _yaw_pid = 0
   _roll_pid = 0
   _pitch_pid = 0
   Led = 0
 End If

End Sub


Sub Send_data()
  _crc = Crc16(_bl(1) , 4)
  Printbin Start_byte
  Incr _btm222_counter
  If _btm222_counter = 20 Then
    _btm222_counter = 0
    If _sbl(_datachannel) > -200 Then
      'Print #2 , _roll_kp ; ":" ; _pitch_kp
      'Print #2 , _roll_ki ; ":" ; _pitch_ki
      'Print #2 , _roll_kd ; ":" ; _pitch_kd
      'Print #2 , _sbl(_throttlechannel) ; ":" ; _sbl(_rollchannel) ; ":" ; _sbl(_pitchchannel) ; ":" ; _sbl(_yawchannel) ; ":" ; _sbl(_statechannel) ; ":" ; _sbl(_datachannel)
    End If
  End If
  Printbin _bl(1) ; _bl(2) ; _bl(3) ; _bl(4) ; _crc
End Sub


Sub Init_system()

_sbl_filter(_throttlechannel) = -1000
_sbl_filter(_rollchannel) = 0
_sbl_filter(_pitchchannel) = 0
_sbl_filter(_yawchannel) = 0
_sbl_filter(_statechannel) = -600
_sbl_filter(_datachannel) = -600

Reset Newvalsreceived

_yawnow = 0
_rollnow = 0
_pitchnow = 0

Call Wmp_init()
Wait 1
For I = 1 To 50
Call Read_wmp_data()
Next I
Wait 1
Call Set_wmp_offset()

For I = 1 To 5
 Led = 1
 Waitms 20
 Led = 0
 Waitms 100
Next I

End Sub

End
Vielen Dank & Gruß
Chris