Da mein erster Post schon zu lange war, muss ich einen zweiten machen.
Hab jetzt mal spaßeshalber den Timer0 (PWM) auskommentiert, dann funktionierts.. Hier der Code:
Code:
$regfile = "m328pdef.dat"
$crystal = 16000000
$framesize = 250
$hwstack = 250
$swstack = 250
$baud = 19200


Config Serialin = Buffered , Size = 50 , Bytematch = 13


Declare Sub Serial0charmatch()

Declare Sub Init_system()

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

Declare Sub Pid_regulator()

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

Declare Sub Failsave()

Declare Sub Set_pwm()



Config Portc.2 = Output
Config Portc.3 = Output
Portc.2 = 0
Portc.3 = 0


Config Portb.1 = Output
Config Portb.2 = Output
Config Portd.5 = Output
Config Portd.6 = Output


Config Timer1 = Pwm , Pwm = 8 , Compare A Pwm = Clear Down , Compare B Pwm = Clear Down , Prescale = 64
Pwm1a = 113
Pwm1b = 113

'Config Timer0 = Pwm , Compare A Pwm = Clear Down , Compare B Pwm = Clear Down , Prescale = 64
'Pwm0a = 113
'Pwm0b = 113


Config Pind.2 = Input
Portd.2 = 0

Config Int0 = Falling
On Int0 Measure
Enable Int0

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


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


Const Minthrottle = 20
Const Maxchannel = 8
Const _throttlechannel = 1
Const _rollchannel = 2
Const _pitchchannel = 3
Const _yawchannel = 4
Const _statechannel = 5
Const _datachannel = 6

Dim I As Byte
Dim J As Byte
Dim Newvalsreceived As Bit
Dim _blink As Byte

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

_yaw_kp = 0.18                                              '0.180
_roll_kp = 0.18
_pitch_kp = 0.18

_yaw_ki = 0
_roll_ki = 0
_pitch_ki = 0

_yaw_kd = 0
_roll_kd = 0
_pitch_kd = 0


'###################################
'#########RC-EMPFÄNGER##############
'###################################
Dim Bufferbyte As Byte
Dim Kanal(maxchannel) As Word
Dim Fkanal(maxchannel) As Word
Dim Channel As Byte
Dim _bl(4) As Word
Dim _sbl(maxchannel) As Integer
Dim State As Byte
Dim Oldstate As Byte
'###################################
'###################################
'###################################


'###################################
'###########I2C-Inputs##############
'###################################
Dim Wmplus_buffer(6) As Byte
Dim Ar(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 _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 As Single
Dim _roll_kd_err As Single
Dim _pitch_kd_err As Single

Dim _yaw_kd_old As Single
Dim _roll_kd_old As Single
Dim _pitch_kd_old 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 Yawstickold As Integer
Dim Rollstickold As Integer
Dim Pitchstickold As Integer

Dim _x1 As Single
Dim _x2 As Single

Dim Rc_on As Word

Dim Failure As Byte



Call Init_system()
Waitms 500
Enable Interrupts
Waitms 500



Rc_test:
If Kanal(_throttlechannel) <= 65 And Kanal(_statechannel) < 80 Then
   If Rc_on < 65000 Then Incr Rc_on
Else
   If Rc_on > 0 Then Decr Rc_on
End If
If Rc_on < 500 Then
   Goto Rc_test
End If




'_bl(1) = rechts hinten  (US)
'_bl(2) = links hinten   (GUS)
'_bl(3) = rechts vorne   (GUS)
'_bl(4) = links vorne    (US)

'PWM = 113 --> 0.900us
'PWM = 250 --> 2.000ms




Do


   Call Filter_rx_data()
   Call Filter_gyro_data()
   Call Pid_regulator()
   Call Mixer()
   Call Failsave()
   Call Set_pwm()


Loop


Sub Serial0charmatch()
   Clear Serialin
End Sub


Sub Set_pwm()
   If _bl(1) > 250 Then _bl(1) = 250
   If _bl(1) < 113 Then _bl(1) = 113
   If _bl(2) > 250 Then _bl(2) = 250
   If _bl(2) < 113 Then _bl(2) = 113
   If _bl(3) > 250 Then _bl(3) = 250
   If _bl(3) < 113 Then _bl(3) = 113
   If _bl(4) > 250 Then _bl(4) = 250
   If _bl(4) < 113 Then _bl(4) = 113
   Pwm0a = _bl(4)                                           'links vorne (US)
   Pwm0b = _bl(2)                                           'links hinten (GUS)
   Pwm1a = _bl(3)                                           'rechts vorne (GUS)
   Pwm1b = _bl(1)                                           'rechts hinten (US)
   If Kanal(_datachannel) > 100 Then
     Print Kanal(1) ; ":" ; Kanal(2) ; ":" ; Kanal(3) ; ":" ; Kanal(4) ; ":" ; Kanal(5)
   End If
End Sub


Sub Failsave()
  If Channel >= 11 Then
    If Failure < 255 Then
      Incr Failure
    End If
  End If
  If Channel < 11 Then
    If Failure > 0 Then
      Decr Failure
    End If
  End If
End Sub


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


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


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

   For I = 1 To Maxchannel
     Fkanal(i) = Fkanal(i) * 7                              '3
     Fkanal(i) = Fkanal(i) + Kanal(i)
     Shift Fkanal(i) , Right , 3                            '2
     Kanal(i) = Fkanal(i)
   Next I

   If Kanal(_throttlechannel) >= 60 And Kanal(_throttlechannel) <= 140 Then
     _sbl(_throttlechannel) = Kanal(_throttlechannel) - 100
     _sbl(_throttlechannel) = _sbl(_throttlechannel) * 2
     If _sbl(_throttlechannel) => 60 Then _sbl(_throttlechannel) = 60
     If _sbl(_throttlechannel) < -68 Then _sbl(_throttlechannel) = -68
   End If

   For I = 2 To Maxchannel
     If Kanal(i) >= 60 And Kanal(i) < 141 Then
       _sbl(i) = Kanal(i) - 100
       If _sbl(i) => -1 And _sbl(i) < 2 Then _sbl(i) = 0
       _sbl(i) = _sbl(i) * 6
     End If
   Next I

   If Kanal(5) < 80 Then State = 0
   If Kanal(5) >= 80 And Kanal(5) < 115 Then State = 1
   If Kanal(5) >= 115 Then State = 2

   If State = 0 Or Oldstate <> State Then
      Rollstickold = 0
      Pitchstickold = 0
   End If

   Yawstickvel = _sbl(_yawchannel) + Yawstickold
   Yawstickold = _sbl(_yawchannel)
   Rollstickvel = _sbl(_rollchannel) + Rollstickold
   Rollstickold = _sbl(_rollchannel)
   Pitchstickvel = _sbl(_pitchchannel) + Pitchstickold
   Pitchstickold = _sbl(_pitchchannel)

   Yawstickvel = Yawstickvel / 4
   Yawstickvel = Yawstickvel * 3

   If State < 2 Then
     Rollstickvel = Rollstickvel / 10
     Pitchstickvel = Pitchstickvel / 10
   Else
     Rollstickvel = Rollstickvel / 4
     Pitchstickvel = Pitchstickvel / 4
   End If

'(
'#########P-GAIN###########
   _x1 = Kanal(7) - 100
   _x1 = _x1 * 0.085
   _x2 = Kanal(8) - 100
   _x2 = _x2 * 0.085

   _roll_kp = 2.80 + _x1
   _pitch_kp = _roll_kp

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

   _roll_ki = 0.0050 + _x1
   _pitch_ki = _roll_ki

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

   _roll_kd = 0.0008 + _x1
   _pitch_kd = _roll_kd
')


   If Kanal(3) > 120 And Kanal(4) > 120 And Kanal(1) < 70 And State = 0 Then
     Call Set_wmp_offset()
   End If

 Oldstate = State

 End If

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 , 4
   Shift Roll , Right , 4
   Shift Pitch , Right , 4
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
   _rolloffset_int = _rolloffset
   _pitchoffset_int = _pitchoffset
End Sub


Sub Pid_regulator()

'##############FEHLER_BERECHNEN##########

 _yaw_err_int = Yawstickvel - _yawnow
 _yaw_err = _yaw_err_int

 _roll_err_int = Rollstickvel - _rollnow
 _roll_err = _roll_err_int

 _pitch_err_int = Pitchstickvel - _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#############

 _yaw_kd_err = _yaw_err * _yaw_kd
 _yaw_kd_err = _yaw_kd_err - _yaw_kd_old
 _yaw_kd_old = _yaw_kd_err

 _roll_kd_err = _roll_err * _roll_kd
 _roll_kd_err = _roll_kd_err - _roll_kd_old
 _roll_kd_old = _roll_kd_err

 _pitch_kd_err = _pitch_err * _pitch_kd
 _pitch_kd_err = _pitch_kd_err - _pitch_kd_old
 _pitch_kd_old = _pitch_kd_err

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

 _yaw_pid = _yaw_kp_err + _yaw_ki_sum
 _yaw_pid = _yaw_pid + _yaw_kd_err
 _yaw_pid_int = _yaw_pid

 _roll_pid = _roll_kp_err + _roll_ki_sum
 _roll_pid = _roll_pid + _roll_kd_err
 _roll_pid_int = _roll_pid

 _pitch_pid = _pitch_kp_err + _pitch_ki_sum
 _pitch_pid = _pitch_pid + _pitch_kd_err
 _pitch_pid_int = _pitch_pid

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

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

End Sub


Sub Mixer()

  '_bl(1) = rechts hinten  (US)
  '_bl(2) = links hinten   (GUS)
  '_bl(3) = rechts vorne   (GUS)
  '_bl(4) = links vorne    (US)

 _bl(1) = 182 + _sbl(_throttlechannel)
 _bl(2) = _bl(1)
 _bl(3) = _bl(1)
 _bl(4) = _bl(1)

 If State <> 0 And Failure < 20 Then

   Portc.2 = 0
   Portc.3 = 1

   _bl(1) = _bl(1) + Minthrottle
   _bl(2) = _bl(2) + Minthrottle
   _bl(3) = _bl(3) + Minthrottle
   _bl(4) = _bl(4) + Minthrottle

   _bl(1) = _bl(1) - _pitch_pid_int
   _bl(2) = _bl(2) - _pitch_pid_int
   _bl(3) = _bl(3) + _pitch_pid_int
   _bl(4) = _bl(4) + _pitch_pid_int

   _bl(1) = _bl(1) + _roll_pid_int
   _bl(2) = _bl(2) - _roll_pid_int
   _bl(3) = _bl(3) + _roll_pid_int
   _bl(4) = _bl(4) - _roll_pid_int

   _bl(1) = _bl(1) - _yaw_pid_int
   _bl(2) = _bl(2) + _yaw_pid_int
   _bl(3) = _bl(3) + _yaw_pid_int
   _bl(4) = _bl(4) - _yaw_pid_int

 Else

   Portc.2 = 0

   _bl(1) = 113
   _bl(2) = 113
   _bl(3) = 113
   _bl(4) = 113

   Incr _blink
   If _blink = 100 Then
     Portc.3 = 1
     _blink = 101
   End If
   If _blink = 200 Then
     Portc.3 = 0
     _blink = 0
   End If

   _yaw_kp_err = 0
   _yaw_ki_sum = 0
   _yaw_kd_err = 0
   _yaw_ki_err = 0
   _roll_kp_err = 0
   _roll_ki_sum = 0
   _roll_kd_err = 0
   _roll_ki_err = 0
   _pitch_kp_err = 0
   _pitch_ki_sum = 0
   _pitch_kd_err = 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
   Yawstickvel = 0
   Rollstickvel = 0
   Pitchstickvel = 0
   Yawstickold = 0
   Rollstickold = 0
   Pitchstickold = 0

 End If

End Sub


Sub Init_system()

_bl(1) = 113
_bl(2) = 113
_bl(3) = 113
_bl(4) = 113

_sbl(1) = -68
_sbl(2) = 0
_sbl(3) = 0
_sbl(4) = 0
_sbl(5) = -600
_sbl(6) = -600
_sbl(7) = 0
_sbl(8) = 0

Fkanal(1) = 63
Fkanal(2) = 100
Fkanal(3) = 100
Fkanal(4) = 100
Fkanal(5) = 63
Fkanal(6) = 63
Fkanal(7) = 100
Fkanal(8) = 100

Reset Newvalsreceived

State = 0
Oldstate = 0

_yawnow = 0
_rollnow = 0
_pitchnow = 0

Call Wmp_init()
Waitms 500
For I = 1 To 10
Call Read_wmp_data()
Next I
Waitms 500
Call Set_wmp_offset()

Rc_on = 0

End Sub

End
Das finde ich sehr merkwürdig! Falls jemand ein paar Tips / Anregungen / sonstiges hat, immer her damit