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:
$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:
$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.
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:
$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:
$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.