Summensignal auslesen funzt nur mit Timer0, nicht mit Timer2!?

**Che Guevara** · 22.09.2011, 15:42

Hallo,

da es mich schon länger gestört hat, auf meinem Quadrocopter 2 Platinen zu benötigen (WMP und Hauptplatine), habe ich mir jetzt eine neue Platine geätzt, auf welcher alle wichtigen Teile vorhanden sind (WMP, ATMEGA328P, RC-Empfänger). Natürlich habe ich vorher die Änderungen nicht ausprobiert, da ich jetzt nur noch einen µC habe und dieser die PWM selbst ausgibt (vorher hat der Master einem ATTINY2313 die 4 Pwm Werte über UART übergeben). Aufgrund dieser Änderung wollte ich jetzt Timer2 zum Auslesen des Summensignals benutzen, jedoch funktioniert das nicht... Habs gerade ausprobiert, wenn ich NUR Timer2 und Timer0 vertausche (einer ist für PWM, der andere fürs Summensignal), gehts.
Hier der Code, der funktioniert:

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 Timer2 = Pwm , Compare A Pwm = Clear Down , Compare B Pwm = Clear Down , Prescale = 64
Pwm2a = 113
Pwm2b = 113


Config Pind.2 = Input
Portd.2 = 0

Config Int0 = Falling
On Int0 Measure
Enable Int0

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


$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
   Pwm2a = _bl(4)                                           'links vorne (US)
   Pwm2b = _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) = Timer0
 End If
 Timer0 = 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

Hier der Code, der NICHT funktioniert:

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

Ist das ein Bug, oder bin ich einfach nur zu blöd, den Unterschied (falls vorhanden) zu bemerken?

Vielen Dank & Gruß
Chris

EDIT:
Habs gerade probiert, "Timer2" durch "TCNT2" zu ersetzen, bringt aber auch nichts... Werd jetzt mal schauen, ob die OVF-ISR (Pausedetect) überhaupt aufgerufen wird.

**Che Guevara** · 23.09.2011, 13:37

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

shedepe · 24.09.2011, 17:22

Du könntest 1. Mal im Datenblatt nachschaun ob sich die beiden Timer irgendwie beeinflussen.
2. Versuchen die Register direkt zusetzen (Also das jeweils richtige Bit im TCNT2 Register und soweiter). Die Register und die zusetzenden Bits findest du auch im Datenblatt.

**Che Guevara** · 25.09.2011, 23:15

Hallo,

im DB habe ich mir schon die meisten Dinge zu den Timern durchgelesen, habe jedoch nichts relevantes gesehen! Hast du einen Tip, wo ich nachsehen sollte?

Morgen Abend werde ich mal versuchen, die Timer über die Register zu konfigurieren. Mal sehen...
Da ich unbedingt fliegen wollte, habe ich heute kurzerhand die 2 Verbindungen vom uC zu den ESC unterbrochen und jeweils ein Kabel direkt vom uC zum ESC verlegt. Jetzt verwende ich den ersten Code, d.h. Timer0 liest das Signal ein und Timer2 generiert die PWMs. Trotzdem möchte ich gerne wissen, warums anders rum nicht funktioniert!

Gruß & Vielen Dank
Chris

**MagicWSmoke** · 26.09.2011, 08:31

Zitat von Che Guevara

Trotzdem möchte ich gerne wissen, warums anders rum nicht funktioniert!

Die Register sind richtig gesetzt, Timer laufen also und Interrupts werden auch ausgelöst, und das in beiden Versionen, im Simulator ist kein Fehler zu entdecken. Ich würde ein HW-Problem vermuten, denn Timer0 hat keine Auswirkung auf Timer2.

Ist denn gewährleistet, daß die PWM von Timer0 ok ist ? Um systematisch vorzugehen würde man erst einmal die Pins PD5..6 in einer Delayloop toggeln. Wenn das geht, den nichtfunktionierenden Code auf das absolute Minimum reduzieren, also Config der Timer, und einen freien Pin in der Pausedetect-ISR toggeln, um auch deren Funktion zu überprüfen.

Wenn ein Oszi vorhanden ist, gut, wenn nicht, dann zeigen normale Multimeter üblicherweise eine Spannung entsprechend des Taktgrades an.

Ab Bascom 2.0.7.1 ist übrigens die Wirkung von ... = Clear Down genau gegenläufig und entspricht nun den Angaben in den Atmel-Datenblättern. Da es jedoch sehr unwahrscheinlich ist, daß die gezeigten Codes mit unterschiedlichen Versionen compiliert wurden, sollte das nicht das Problem sein.

**Che Guevara** · 26.09.2011, 14:07

Hallo,

PWM funktioniert von allen 3 Timern richtig! Oszi ist vorhanden, mit diesem habe ich auch schon die Funktionstüchtigkeit der PWMs überprüft.
Ich habe leider momentan keine Vermutung, wo das Problem liegen könnte. Werd aber heute Abend mal ein paar Tests durchführen.

Gruß
Chris

**MagicWSmoke** · 26.09.2011, 15:08

Zitat von Che Guevara

PWM funktioniert von allen 3 Timern richtig! Oszi ist vorhanden, mit diesem habe ich auch schon die Funktionstüchtigkeit der PWMs überprüft.

Nachdem ich die Timer alle 3 im AVR-Studio laufen sehe (zum Testen die Prescaler verkleinert) und auch der Interrupt auf Pausedetect richtig kommt, bleiben nicht so viele Möglichkeiten über.

Voraussetzung ist, daß der nichtfunktionierende Code gegenüber dem Funktionierenden nur im Bereich der Timer-Config, bzw. Zuweisung der Arrayelemente _bl 4,2 verändert wurde und sich keine weiteren versteckten Zugriffe auf den Timer im Code versteckt halten.
Fand beim Drübersehen zumindest keine.

Eine Fehlermöglichkeit wäre sicher eine ältere Bascom-Version, die Timer2 nicht richtig setzt. Was für eine Version wird genutzt ? Ich kann an der erzeugte .hex-Datei erkennen, ob da was falsch gesetzt wird, kannst ja mal anhängen.

**Che Guevara** · 26.09.2011, 15:31

Die Codes sind bis auf die Timer-Configs komplett gleich. Die Timer-Anweisungen kommen nur in Pausedetect und in Measure vor.
Bascom Version ist die 1.11.9.5. Hex File ist im Anhand, danke

Gruß
Chris

**MagicWSmoke** · 26.09.2011, 19:57

Also Timer2 in dem Hex läuft, die Register sind ordentlich gesetzt und die ISR wird aufgerufen, wenn ich das I-Flag im SREG manuell setze. Bis zum Enable Interrupts geht die Simulation nicht, denn die Waits blockieren, später auch noch das I2C, ohne I2C-HW geht da nicht viel.

Ich kann mir immer noch vorstellen, daß es ein HW-Problem ist, denn die Timer beeinflussen sich intern nicht, außer einer schreibt dem anderen die Register kaputt, da muss aber zusätzlicher Code im Spiel sein. Kommentier' doch mal den Timer0 als PWM aus und setz' OC0A..B auf Ausgang und 1, oder toggle OC0A..B in der Mainloop.

Anbei ein per 2.0.7.0 kompiliertes Hex, sollte das laufen, wird's Zeit für ein Update.

**Che Guevara** · 26.09.2011, 20:43

Erstmal danke fürs drüberschauen

Aber eine Frage hätte ich: Wie kannst du überprüfen, ob das Hex file korrekt ist?
Ein HW-Problem kann ich ausschließen, den wenn ich Timer0 (als PWM) auskommentiere, funzt Timer2 ... PWM von Timer0 funzt auch, wenn ich z.b. fest vorgebene Werte in die Register schreibe (PWM0a und PWM0b) werden diese korrekt ausgegeben (mit dem Oszi und Motoren überprüft). Lediglich beide Timer zusammen (Timer0 als PWM; Timer2 fürs Summensignal) funktionieren nicht.
Das Hex File werde ich morgen Abend mal draufspielen, werde dann berichten, obs geklappt hat. Vielen Dank schonmal.

Gruß
Chris