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:
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 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
Ist das ein Bug, oder bin ich einfach nur zu blöd, den Unterschied (falls vorhanden) zu bemerken?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
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.
Zitieren
Lesezeichen