Che Guevara
05.07.2011, 21:37
Hallo,
immer wenn ich mit meinem Tricopter fliege, passiert es manchmal während des Fliegens, dass er kurz (ca. 100ms - geschätzt) schlagartig nach unten fällt (d.h. die Motoren setzen alle gleichzeitig aus). Nach ca. 1-2Meter freien Fallens starten die Motoren wieder und er fängt sich. Glücklicherweise bin ich bis jetzt immer so hoch geflogen, dass er sich immer ca. 2m über dem Boden wieder stabilisiert hat. Trotzdem ärgert es mich doch sehr, da dadurch der Spaß einwenig flöten geht und es nicht schön aussieht!
Zur Hardware:
- ATMEGA328P als Haupt- und Rechencontroller (@16MHz)
--> er liest den Sensor (WM+) und die Funke (Graupner Hott MX-16 2,4GHz) aus und errechnet daraus die PID-Werte, welche er dann über UART (115.200Baud) zum Slave schickt
- ATTINY2313 als PWM-Controller (@16MHz)
--> er empfängt die Daten samt Start-Byte und CRC-Checksumme und synchronisiert sich immer selbst (hängenbleiben ausgeschlossen) und gibt sie dann einfach stur auf die 4 PWM Ausgänge (Software PWM) aus
- WM+ als Gyro, kein ACC vorhanden
- BTM222 Funkmodul
--> mit diesem können Daten an einen AVR gesendet werden, welcher diese an den PC sendet (momentan nicht benutzt)
Aktoren:
- 3x CF2822 (Emax) Motoren mit 10x4.7 Props und Graupner Brushless-Control 18A
--> Die Motoren und Regler werden spürbar warm, IMHO aber nicht heiß
- 1x Servo (Standart Servo von Conrad für 2.95€)
Akku:
- Graupner 3S/2600mAh Lipo
Programmiert wurde alles in Bascom. Der Regler läuft momentan mit ca. 400Hz (wenn ich mich nicht verrechnet habe). Ich glaube (!!!), dass das Problem nicht auftratt, als der Regler noch mit ca. 130Hz (also mit 38400Baud) lief. Sicher bin ich mir aber da keinesfalls, v.a. weil damals die Regelung noch nicht richtig funktioniert hat und er sich nicht 100% stabilisierte.
Die Software:
SLAVE (ATTINY2313)
$regfile = "attiny2313.dat"
$crystal = 16000000
$framesize = 30
$hwstack = 32
$swstack = 30
$baud = 115200 '38400
Config Timer1 = Timer , Prescale = 8
Timer1 = 62535
On Timer1 Servoirq
Enable Timer1
Config Portd.2 = Output
Config Portd.3 = Output
Config Portd.4 = Output
Config Portd.5 = Output
Dim Kanal As Byte
Dim _servo(4) As Word
Dim _bl(4) As Word
Dim I As Word
Dim _crc As Word
Dim _crcold As Word
Dim _start As Byte
'min: 63800, mitte: 62667.5 , max: 61535 --> 2265 Schritte
Const Start_byte = 127
Const Min_servo = 63800
Const Max_servo = 61535
Const Diff_servo = Max_servo - Min_servo
For I = 1 To 3
_bl(i) = Min_servo
_servo(i) = _bl(i)
Next I
_bl(4) = 63200
_servo(4) = _bl(4)
Enable Interrupts
Wait 3
Do
If Ischarwaiting() > 0 Then
Inputbin _start
If _start = Start_byte Then
Inputbin _bl(1) , _bl(2) , _bl(3) , _bl(4) , _crc
If _crc = Crc16(_bl(1) , 4) Then 'And _crc <> _crcold Then
For I = 1 To 4
If _bl(i) < Max_servo Then _bl(i) = Max_servo
If _bl(i) > Min_servo Then _bl(i) = Min_servo
_servo(i) = _bl(i)
Next I
End If
End If
End If
Loop
Servoirq:
If Kanal = 0 Then
If Portd.2 = 0 Then 'wenn port low
Timer1 = _servo(1) 'dann timer auf entsprechende verzögerung
Portd.2 = 1 'und port anschalten
Else 'das hier passiert erst bei dem darauf folgenden interrupt
Portd.2 = 0 'dann port wieder ausschalten
Incr Kanal 'und den nächsten kanal bearbeiten
End If
End If
If Kanal = 1 Then
If Portd.3 = 0 Then
Timer1 = _servo(2)
Portd.3 = 1
Else
Portd.3 = 0
Incr Kanal
End If
End If
If Kanal = 2 Then
If Portd.4 = 0 Then
Timer1 = _servo(3)
Portd.4 = 1
Else
Portd.4 = 0
Incr Kanal
End If
End If
If Kanal = 3 Then
If Portd.5 = 0 Then
Timer1 = _servo(4)
Portd.5 = 1
Else
Portd.5 = 0
Incr Kanal
End If
End If
If Kanal = 4 Then
Timer1 = 40000 '65535 '40000 | eine pause von ca. 12ms bis zum nächsten interrupt. Bei guten Servos oder Brushlessreglern kann man hier bis auf 65530 gehen ==> ansteuerfrequenz von ~ 200Hz
Kanal = 0
End If
Return
End
MASTER (ATMEGA328P)
$regfile = "m328pdef.dat"
$crystal = 16000000
$framesize = 200
$hwstack = 200
$swstack = 200
$baud = 115200
Open "COMC.2:19200,8,N,1" For Input As #1
Open "COMC.3:19200,8,N,1" For Output As #2
Declare Sub Init_system()
Declare Sub Filter_gyro_data()
Declare Sub Filter_rx_data()
Declare Sub Mixer()
Declare Sub Send_data()
Declare Sub Pid_regulator()
Declare Sub Wmp_init()
Declare Sub Send_zero()
Declare Sub Read_wmp_data()
Declare Sub Set_wmp_offset()
$lib "I2C_TWI.LBX"
Config Scl = Portc.5
Config Sda = Portc.4
Config Twi = 100000
I2cinit
Config Timer0 = Timer , Prescale = 256
On Timer0 Pausedetect
Enable Timer0
Config Int0 = Falling
On Int0 Measure
Enable Int0
Config Pind.2 = Input
Portd.2 = 0
'####################################
'###########KONSTANTEN###############
'####################################
Const _maxchannel = 8
Const Start_byte = 127
Dim _yaw_kp As Integer
Dim _roll_kp As Integer
Dim _pitch_kp As Integer
Dim _yaw_ki As Integer
Dim _roll_ki As Integer
Dim _pitch_ki As Integer
Dim _yaw_kd As Integer
Dim _roll_kd As Integer
Dim _pitch_kd As Integer
_yaw_kp = 80 '80 evtl. größer
_roll_kp = 30 '30
_pitch_kp = 85 '85
_yaw_ki = 60 '60 evtl. größer
_roll_ki = 90 '90
_pitch_ki = 138 '138
_yaw_kd = 0 '0
_roll_kd = 120 '120 evtl. größer
_pitch_kd = 112 '112 evtl. größer
Const _bl1offset = 0
Const _bl2offset = 0
Const _bl3offset = 0
Const _bl4offset = 0
'###################################
'###################################
'###################################
Dim _long_tmp As Long
Dim _timer2_counter As Byte
'###################################
'#########RC-EMPFÄNGER##############
'###################################
Dim Bufferbyte As Byte
Dim Kanal(_maxchannel) As Word
Dim Channel As Byte
Dim _bl(4) As Word
Dim I As Byte
Dim _crc As Word
Dim _sbl(_maxchannel) As Integer
'###################################
'###################################
'###################################
'###################################
'###########I2C-Inputs##############
'###################################
Dim Wmplus_buffer(6) As Byte
'###################################
'###################################
'###################################
'###################################
'#########GYRO######################
'###################################
Dim Yaw As Word
Dim Yaw0 As Byte At Yaw + 1 Overlay
Dim Yaw1 As Byte At Yaw Overlay
Dim Roll As Word
Dim Roll0 As Byte At Roll + 1 Overlay
Dim Roll1 As Byte At Roll Overlay
Dim Pitch As Word
Dim Pitch0 As Byte At Pitch + 1 Overlay
Dim Pitch1 As Byte At Pitch Overlay
Dim _yawoffset As Long
Dim _rolloffset As Long
Dim _pitchoffset As Long
Dim _yawoffset_int As Integer
Dim _rolloffset_int As Integer
Dim _pitchoffset_int As Integer
Dim _yawnow As Integer
Dim _rollnow As Integer
Dim _pitchnow As Integer
'###################################
'###################################
'###################################
'##################################
'#########PID-REGLER###############
'##################################
Dim _yaw_kp_err As Integer
Dim _roll_kp_err As Integer
Dim _pitch_kp_err As Integer
Dim _yaw_ki_err As Integer
Dim _roll_ki_err As Integer
Dim _pitch_ki_err As Integer
Dim _yaw_ki_sum As Integer
Dim _roll_ki_sum As Integer
Dim _pitch_ki_sum As Integer
Dim _yaw_kd_err As Integer
Dim _roll_kd_err As Integer
Dim _pitch_kd_err As Integer
Dim _yaw_kd_old As Integer
Dim _roll_kd_old As Integer
Dim _pitch_kd_old As Integer
Dim _yaw_pid As Integer
Dim _roll_pid As Integer
Dim _pitch_pid As Integer
Dim _yaw_err As Integer
Dim _roll_err As Integer
Dim _pitch_err As Integer
'#################################
'#################################
'#################################
'#################################
'#############FILTER##############
'#################################
Dim _sbl_old As Integer
'#################################
'#################################
'#################################
Wait 2
Call Init_system()
Enable Interrupts
Do
Call Filter_rx_data() '400
Call Filter_gyro_data() '1910
Call Pid_regulator() '1000
Call Mixer() '39
Call Send_data() '10000
Loop
Sub Filter_gyro_data()
Call Read_wmp_data()
_yawnow = Yaw - _yawoffset_int
_rollnow = Roll - _rolloffset_int
_pitchnow = Pitch - _pitchoffset_int
End Sub
Sub Filter_rx_data()
For I = 1 To _maxchannel
_sbl(i) = Kanal(i) - 100
If _sbl(i) < 2 And _sbl(i) > -2 Then _sbl(i) = 0
_sbl(i) = _sbl(i) * 25
Next I
If _sbl(5) > -200 And _sbl(5) < 200 Then
_sbl(2) = _sbl(2) / 6
_sbl(3) = _sbl(3) / 6
Elseif _sbl(5) > 200 Then
_sbl(2) = _sbl(2) / 3
_sbl(3) = _sbl(3) / 3
End If
_sbl(4) = _sbl(4) * 10
_sbl(4) = _sbl(4) / 20
End Sub
Measure:
If Channel > 0 And Channel < 9 Then
Kanal(channel) = Timer0
End If
Timer0 = 6
Incr Channel
Return
Pausedetect:
Channel = 0
Return
Sub Wmp_init()
I2cstart
I2cwbyte &HA6
I2cwbyte &HFE
I2cwbyte &H04
I2cstop
End Sub
Sub Send_zero()
I2cstart
I2cwbyte &HA4
I2cwbyte &H00
I2cstop
End Sub
Sub Read_wmp_data()
Gosub Send_zero
I2creceive &HA4 , Wmplus_buffer(1) , 0 , 6
Yaw1 = Wmplus_buffer(1)
Roll1 = Wmplus_buffer(2)
Pitch1 = Wmplus_buffer(3)
Shift Wmplus_buffer(4) , Right , 2 : Yaw0 = Wmplus_buffer(4)
Shift Wmplus_buffer(5) , Right , 2 : Roll0 = Wmplus_buffer(5)
Shift Wmplus_buffer(6) , Right , 2 : Pitch0 = Wmplus_buffer(6)
Shift Yaw , Right , 2
Shift Roll , Right , 2
Shift Pitch , Right , 2
End Sub
Sub Set_wmp_offset()
_yawoffset = 0
_rolloffset = 0
_pitchoffset = 0
For I = 1 To 100
Waitms 5
Call Read_wmp_data()
_yawoffset = _yawoffset + Yaw
_rolloffset = _rolloffset + Roll
_pitchoffset = _pitchoffset + Pitch
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 = _sbl(4) - _yawnow
_roll_err = _sbl(2) - _rollnow
_pitch_err = _sbl(3) - _pitchnow
'##############PROPORTIONAL##############
_long_tmp = _yaw_err * _yaw_kp
_long_tmp = _long_tmp / 200
_yaw_kp_err = _long_tmp
_long_tmp = _roll_err * _roll_kp
_long_tmp = _long_tmp / 200
_roll_kp_err = _long_tmp
_long_tmp = _pitch_err * _pitch_kp
_long_tmp = _long_tmp / 200
_pitch_kp_err = _long_tmp
'##############INTEGRAL##################
_long_tmp = _yaw_err * _yaw_ki
_long_tmp = _long_tmp / 20000
_yaw_ki_err = _long_tmp
_yaw_ki_sum = _yaw_ki_sum + _yaw_ki_err
_long_tmp = _roll_err * _roll_ki
_long_tmp = _long_tmp / 20000
_roll_ki_err = _long_tmp
_roll_ki_sum = _roll_ki_sum + _roll_ki_err
_long_tmp = _pitch_err * _pitch_ki
_long_tmp = _long_tmp / 20000
_pitch_ki_err = _long_tmp
_pitch_ki_sum = _pitch_ki_sum + _pitch_ki_err
'##############DIFFERENNTIAL#############
_long_tmp = _yaw_err * _yaw_kd
_long_tmp = _long_tmp / 40000
_yaw_kd_err = _long_tmp
_yaw_kd_err = _yaw_kd_old - _yaw_kd_err
_yaw_kd_old = _yaw_kd_err
_long_tmp = _roll_err * _roll_kd
_long_tmp = _long_tmp / 40000
_roll_kd_err = _long_tmp
_roll_kd_err = _roll_kd_old - _roll_kd_err
_roll_kd_old = _roll_kd_err
_long_tmp = _pitch_err * _pitch_kd
_long_tmp = _long_tmp / 40000
_pitch_kd_err = _long_tmp
_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
_roll_pid = _roll_kp_err + _roll_ki_sum
_roll_pid = _roll_pid + _roll_kd_err
_pitch_pid = _pitch_kp_err + _pitch_ki_sum
_pitch_pid = _pitch_pid + _pitch_kd_err
'###############WERTE_BEGRENZEN##########
If _yaw_pid < -1000 Then _yaw_pid = -1000
If _yaw_pid > 1000 Then _yaw_pid = 1000
If _roll_pid < -1000 Then _roll_pid = -1000
If _roll_pid > 1000 Then _roll_pid = 1000
If _pitch_pid < -1000 Then _pitch_pid = -1000
If _pitch_pid > 1000 Then _pitch_pid = 1000
End Sub
Sub Mixer()
_bl(1) = 62667 - _sbl(1)
_bl(2) = _bl(1)
_bl(3) = _bl(1)
_bl(4) = 62667
_bl(1) = _bl(1) + _bl1offset
_bl(2) = _bl(2) + _bl2offset
_bl(3) = _bl(3) + _bl3offset
_bl(4) = _bl(4) + _bl4offset
If _sbl(5) > -200 Then
_bl(1) = _bl(1) + _pitch_pid
_bl(2) = _bl(2) - _roll_pid
_bl(3) = _bl(3) + _roll_pid
_pitch_pid = _pitch_pid / 2
_bl(2) = _bl(2) - _pitch_pid
_bl(3) = _bl(3) - _pitch_pid
_bl(4) = _bl(4) - _yaw_pid
Elseif _sbl(5) < -200 And _sbl_old < -200 Then
_bl(1) = 63800
_bl(2) = 63800
_bl(3) = 63800
_yaw_kp_err = 0
_yaw_ki_sum = 0
_yaw_kd_err = 0
_yaw_ki_err = 0
_yaw_kd_old = 0
_roll_kp_err = 0
_roll_ki_sum = 0
_roll_kd_err = 0
_roll_ki_err = 0
_roll_kd_old = 0
_pitch_kp_err = 0
_pitch_ki_sum = 0
_pitch_kd_err = 0
_pitch_ki_err = 0
_pitch_kd_old = 0
_yaw_pid = 0
_roll_pid = 0
_pitch_pid = 0
End If
_sbl_old = _sbl(5)
End Sub
Sub Send_data()
_crc = Crc16(_bl(1) , 4)
Printbin Start_byte ; _bl(1) ; _bl(2) ; _bl(3) ; _bl(4) ; _crc
End Sub
Sub Init_system()
_yawnow = 0
_rollnow = 0
_pitchnow = 0
Call Wmp_init()
Wait 1
For I = 1 To 50 'the first measurements are trash!
Call Read_wmp_data()
Next I
Wait 1
Call Set_wmp_offset()
End Sub
End
Ich habe schon lange gesucht, aber keinen Fehler gefunden! Der Code ist noch nicht geschönt und optimiert, aber er funktioniert gut.
Vielleicht weiß ja jemand von euch, wie ich den Fehler finden köntte oder ihm fällt jetzt schon was auf.. Ich wäre sehr dankbar, da mich das doch sehr stört.
Vielen Dank & Gruß
Chris
EDIT:
Weiß jemand zufällig, wie man das Sinken des Copters bei zu starker Schrägstellung einwenig kompensieren kann? Ich denke, das müsste doch irgendwie mittels der I-Anteile der 2 Regler (Pitch und Roll) funktionieren?! Sollte ich es mal probieren, diese jeweils in positiver Form auf alle Motoren zu geben? Wie würdet ihr soetwas machen? Oder ist es üblich, dass der Pilot das durch mehr Gas kompensiert?
immer wenn ich mit meinem Tricopter fliege, passiert es manchmal während des Fliegens, dass er kurz (ca. 100ms - geschätzt) schlagartig nach unten fällt (d.h. die Motoren setzen alle gleichzeitig aus). Nach ca. 1-2Meter freien Fallens starten die Motoren wieder und er fängt sich. Glücklicherweise bin ich bis jetzt immer so hoch geflogen, dass er sich immer ca. 2m über dem Boden wieder stabilisiert hat. Trotzdem ärgert es mich doch sehr, da dadurch der Spaß einwenig flöten geht und es nicht schön aussieht!
Zur Hardware:
- ATMEGA328P als Haupt- und Rechencontroller (@16MHz)
--> er liest den Sensor (WM+) und die Funke (Graupner Hott MX-16 2,4GHz) aus und errechnet daraus die PID-Werte, welche er dann über UART (115.200Baud) zum Slave schickt
- ATTINY2313 als PWM-Controller (@16MHz)
--> er empfängt die Daten samt Start-Byte und CRC-Checksumme und synchronisiert sich immer selbst (hängenbleiben ausgeschlossen) und gibt sie dann einfach stur auf die 4 PWM Ausgänge (Software PWM) aus
- WM+ als Gyro, kein ACC vorhanden
- BTM222 Funkmodul
--> mit diesem können Daten an einen AVR gesendet werden, welcher diese an den PC sendet (momentan nicht benutzt)
Aktoren:
- 3x CF2822 (Emax) Motoren mit 10x4.7 Props und Graupner Brushless-Control 18A
--> Die Motoren und Regler werden spürbar warm, IMHO aber nicht heiß
- 1x Servo (Standart Servo von Conrad für 2.95€)
Akku:
- Graupner 3S/2600mAh Lipo
Programmiert wurde alles in Bascom. Der Regler läuft momentan mit ca. 400Hz (wenn ich mich nicht verrechnet habe). Ich glaube (!!!), dass das Problem nicht auftratt, als der Regler noch mit ca. 130Hz (also mit 38400Baud) lief. Sicher bin ich mir aber da keinesfalls, v.a. weil damals die Regelung noch nicht richtig funktioniert hat und er sich nicht 100% stabilisierte.
Die Software:
SLAVE (ATTINY2313)
$regfile = "attiny2313.dat"
$crystal = 16000000
$framesize = 30
$hwstack = 32
$swstack = 30
$baud = 115200 '38400
Config Timer1 = Timer , Prescale = 8
Timer1 = 62535
On Timer1 Servoirq
Enable Timer1
Config Portd.2 = Output
Config Portd.3 = Output
Config Portd.4 = Output
Config Portd.5 = Output
Dim Kanal As Byte
Dim _servo(4) As Word
Dim _bl(4) As Word
Dim I As Word
Dim _crc As Word
Dim _crcold As Word
Dim _start As Byte
'min: 63800, mitte: 62667.5 , max: 61535 --> 2265 Schritte
Const Start_byte = 127
Const Min_servo = 63800
Const Max_servo = 61535
Const Diff_servo = Max_servo - Min_servo
For I = 1 To 3
_bl(i) = Min_servo
_servo(i) = _bl(i)
Next I
_bl(4) = 63200
_servo(4) = _bl(4)
Enable Interrupts
Wait 3
Do
If Ischarwaiting() > 0 Then
Inputbin _start
If _start = Start_byte Then
Inputbin _bl(1) , _bl(2) , _bl(3) , _bl(4) , _crc
If _crc = Crc16(_bl(1) , 4) Then 'And _crc <> _crcold Then
For I = 1 To 4
If _bl(i) < Max_servo Then _bl(i) = Max_servo
If _bl(i) > Min_servo Then _bl(i) = Min_servo
_servo(i) = _bl(i)
Next I
End If
End If
End If
Loop
Servoirq:
If Kanal = 0 Then
If Portd.2 = 0 Then 'wenn port low
Timer1 = _servo(1) 'dann timer auf entsprechende verzögerung
Portd.2 = 1 'und port anschalten
Else 'das hier passiert erst bei dem darauf folgenden interrupt
Portd.2 = 0 'dann port wieder ausschalten
Incr Kanal 'und den nächsten kanal bearbeiten
End If
End If
If Kanal = 1 Then
If Portd.3 = 0 Then
Timer1 = _servo(2)
Portd.3 = 1
Else
Portd.3 = 0
Incr Kanal
End If
End If
If Kanal = 2 Then
If Portd.4 = 0 Then
Timer1 = _servo(3)
Portd.4 = 1
Else
Portd.4 = 0
Incr Kanal
End If
End If
If Kanal = 3 Then
If Portd.5 = 0 Then
Timer1 = _servo(4)
Portd.5 = 1
Else
Portd.5 = 0
Incr Kanal
End If
End If
If Kanal = 4 Then
Timer1 = 40000 '65535 '40000 | eine pause von ca. 12ms bis zum nächsten interrupt. Bei guten Servos oder Brushlessreglern kann man hier bis auf 65530 gehen ==> ansteuerfrequenz von ~ 200Hz
Kanal = 0
End If
Return
End
MASTER (ATMEGA328P)
$regfile = "m328pdef.dat"
$crystal = 16000000
$framesize = 200
$hwstack = 200
$swstack = 200
$baud = 115200
Open "COMC.2:19200,8,N,1" For Input As #1
Open "COMC.3:19200,8,N,1" For Output As #2
Declare Sub Init_system()
Declare Sub Filter_gyro_data()
Declare Sub Filter_rx_data()
Declare Sub Mixer()
Declare Sub Send_data()
Declare Sub Pid_regulator()
Declare Sub Wmp_init()
Declare Sub Send_zero()
Declare Sub Read_wmp_data()
Declare Sub Set_wmp_offset()
$lib "I2C_TWI.LBX"
Config Scl = Portc.5
Config Sda = Portc.4
Config Twi = 100000
I2cinit
Config Timer0 = Timer , Prescale = 256
On Timer0 Pausedetect
Enable Timer0
Config Int0 = Falling
On Int0 Measure
Enable Int0
Config Pind.2 = Input
Portd.2 = 0
'####################################
'###########KONSTANTEN###############
'####################################
Const _maxchannel = 8
Const Start_byte = 127
Dim _yaw_kp As Integer
Dim _roll_kp As Integer
Dim _pitch_kp As Integer
Dim _yaw_ki As Integer
Dim _roll_ki As Integer
Dim _pitch_ki As Integer
Dim _yaw_kd As Integer
Dim _roll_kd As Integer
Dim _pitch_kd As Integer
_yaw_kp = 80 '80 evtl. größer
_roll_kp = 30 '30
_pitch_kp = 85 '85
_yaw_ki = 60 '60 evtl. größer
_roll_ki = 90 '90
_pitch_ki = 138 '138
_yaw_kd = 0 '0
_roll_kd = 120 '120 evtl. größer
_pitch_kd = 112 '112 evtl. größer
Const _bl1offset = 0
Const _bl2offset = 0
Const _bl3offset = 0
Const _bl4offset = 0
'###################################
'###################################
'###################################
Dim _long_tmp As Long
Dim _timer2_counter As Byte
'###################################
'#########RC-EMPFÄNGER##############
'###################################
Dim Bufferbyte As Byte
Dim Kanal(_maxchannel) As Word
Dim Channel As Byte
Dim _bl(4) As Word
Dim I As Byte
Dim _crc As Word
Dim _sbl(_maxchannel) As Integer
'###################################
'###################################
'###################################
'###################################
'###########I2C-Inputs##############
'###################################
Dim Wmplus_buffer(6) As Byte
'###################################
'###################################
'###################################
'###################################
'#########GYRO######################
'###################################
Dim Yaw As Word
Dim Yaw0 As Byte At Yaw + 1 Overlay
Dim Yaw1 As Byte At Yaw Overlay
Dim Roll As Word
Dim Roll0 As Byte At Roll + 1 Overlay
Dim Roll1 As Byte At Roll Overlay
Dim Pitch As Word
Dim Pitch0 As Byte At Pitch + 1 Overlay
Dim Pitch1 As Byte At Pitch Overlay
Dim _yawoffset As Long
Dim _rolloffset As Long
Dim _pitchoffset As Long
Dim _yawoffset_int As Integer
Dim _rolloffset_int As Integer
Dim _pitchoffset_int As Integer
Dim _yawnow As Integer
Dim _rollnow As Integer
Dim _pitchnow As Integer
'###################################
'###################################
'###################################
'##################################
'#########PID-REGLER###############
'##################################
Dim _yaw_kp_err As Integer
Dim _roll_kp_err As Integer
Dim _pitch_kp_err As Integer
Dim _yaw_ki_err As Integer
Dim _roll_ki_err As Integer
Dim _pitch_ki_err As Integer
Dim _yaw_ki_sum As Integer
Dim _roll_ki_sum As Integer
Dim _pitch_ki_sum As Integer
Dim _yaw_kd_err As Integer
Dim _roll_kd_err As Integer
Dim _pitch_kd_err As Integer
Dim _yaw_kd_old As Integer
Dim _roll_kd_old As Integer
Dim _pitch_kd_old As Integer
Dim _yaw_pid As Integer
Dim _roll_pid As Integer
Dim _pitch_pid As Integer
Dim _yaw_err As Integer
Dim _roll_err As Integer
Dim _pitch_err As Integer
'#################################
'#################################
'#################################
'#################################
'#############FILTER##############
'#################################
Dim _sbl_old As Integer
'#################################
'#################################
'#################################
Wait 2
Call Init_system()
Enable Interrupts
Do
Call Filter_rx_data() '400
Call Filter_gyro_data() '1910
Call Pid_regulator() '1000
Call Mixer() '39
Call Send_data() '10000
Loop
Sub Filter_gyro_data()
Call Read_wmp_data()
_yawnow = Yaw - _yawoffset_int
_rollnow = Roll - _rolloffset_int
_pitchnow = Pitch - _pitchoffset_int
End Sub
Sub Filter_rx_data()
For I = 1 To _maxchannel
_sbl(i) = Kanal(i) - 100
If _sbl(i) < 2 And _sbl(i) > -2 Then _sbl(i) = 0
_sbl(i) = _sbl(i) * 25
Next I
If _sbl(5) > -200 And _sbl(5) < 200 Then
_sbl(2) = _sbl(2) / 6
_sbl(3) = _sbl(3) / 6
Elseif _sbl(5) > 200 Then
_sbl(2) = _sbl(2) / 3
_sbl(3) = _sbl(3) / 3
End If
_sbl(4) = _sbl(4) * 10
_sbl(4) = _sbl(4) / 20
End Sub
Measure:
If Channel > 0 And Channel < 9 Then
Kanal(channel) = Timer0
End If
Timer0 = 6
Incr Channel
Return
Pausedetect:
Channel = 0
Return
Sub Wmp_init()
I2cstart
I2cwbyte &HA6
I2cwbyte &HFE
I2cwbyte &H04
I2cstop
End Sub
Sub Send_zero()
I2cstart
I2cwbyte &HA4
I2cwbyte &H00
I2cstop
End Sub
Sub Read_wmp_data()
Gosub Send_zero
I2creceive &HA4 , Wmplus_buffer(1) , 0 , 6
Yaw1 = Wmplus_buffer(1)
Roll1 = Wmplus_buffer(2)
Pitch1 = Wmplus_buffer(3)
Shift Wmplus_buffer(4) , Right , 2 : Yaw0 = Wmplus_buffer(4)
Shift Wmplus_buffer(5) , Right , 2 : Roll0 = Wmplus_buffer(5)
Shift Wmplus_buffer(6) , Right , 2 : Pitch0 = Wmplus_buffer(6)
Shift Yaw , Right , 2
Shift Roll , Right , 2
Shift Pitch , Right , 2
End Sub
Sub Set_wmp_offset()
_yawoffset = 0
_rolloffset = 0
_pitchoffset = 0
For I = 1 To 100
Waitms 5
Call Read_wmp_data()
_yawoffset = _yawoffset + Yaw
_rolloffset = _rolloffset + Roll
_pitchoffset = _pitchoffset + Pitch
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 = _sbl(4) - _yawnow
_roll_err = _sbl(2) - _rollnow
_pitch_err = _sbl(3) - _pitchnow
'##############PROPORTIONAL##############
_long_tmp = _yaw_err * _yaw_kp
_long_tmp = _long_tmp / 200
_yaw_kp_err = _long_tmp
_long_tmp = _roll_err * _roll_kp
_long_tmp = _long_tmp / 200
_roll_kp_err = _long_tmp
_long_tmp = _pitch_err * _pitch_kp
_long_tmp = _long_tmp / 200
_pitch_kp_err = _long_tmp
'##############INTEGRAL##################
_long_tmp = _yaw_err * _yaw_ki
_long_tmp = _long_tmp / 20000
_yaw_ki_err = _long_tmp
_yaw_ki_sum = _yaw_ki_sum + _yaw_ki_err
_long_tmp = _roll_err * _roll_ki
_long_tmp = _long_tmp / 20000
_roll_ki_err = _long_tmp
_roll_ki_sum = _roll_ki_sum + _roll_ki_err
_long_tmp = _pitch_err * _pitch_ki
_long_tmp = _long_tmp / 20000
_pitch_ki_err = _long_tmp
_pitch_ki_sum = _pitch_ki_sum + _pitch_ki_err
'##############DIFFERENNTIAL#############
_long_tmp = _yaw_err * _yaw_kd
_long_tmp = _long_tmp / 40000
_yaw_kd_err = _long_tmp
_yaw_kd_err = _yaw_kd_old - _yaw_kd_err
_yaw_kd_old = _yaw_kd_err
_long_tmp = _roll_err * _roll_kd
_long_tmp = _long_tmp / 40000
_roll_kd_err = _long_tmp
_roll_kd_err = _roll_kd_old - _roll_kd_err
_roll_kd_old = _roll_kd_err
_long_tmp = _pitch_err * _pitch_kd
_long_tmp = _long_tmp / 40000
_pitch_kd_err = _long_tmp
_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
_roll_pid = _roll_kp_err + _roll_ki_sum
_roll_pid = _roll_pid + _roll_kd_err
_pitch_pid = _pitch_kp_err + _pitch_ki_sum
_pitch_pid = _pitch_pid + _pitch_kd_err
'###############WERTE_BEGRENZEN##########
If _yaw_pid < -1000 Then _yaw_pid = -1000
If _yaw_pid > 1000 Then _yaw_pid = 1000
If _roll_pid < -1000 Then _roll_pid = -1000
If _roll_pid > 1000 Then _roll_pid = 1000
If _pitch_pid < -1000 Then _pitch_pid = -1000
If _pitch_pid > 1000 Then _pitch_pid = 1000
End Sub
Sub Mixer()
_bl(1) = 62667 - _sbl(1)
_bl(2) = _bl(1)
_bl(3) = _bl(1)
_bl(4) = 62667
_bl(1) = _bl(1) + _bl1offset
_bl(2) = _bl(2) + _bl2offset
_bl(3) = _bl(3) + _bl3offset
_bl(4) = _bl(4) + _bl4offset
If _sbl(5) > -200 Then
_bl(1) = _bl(1) + _pitch_pid
_bl(2) = _bl(2) - _roll_pid
_bl(3) = _bl(3) + _roll_pid
_pitch_pid = _pitch_pid / 2
_bl(2) = _bl(2) - _pitch_pid
_bl(3) = _bl(3) - _pitch_pid
_bl(4) = _bl(4) - _yaw_pid
Elseif _sbl(5) < -200 And _sbl_old < -200 Then
_bl(1) = 63800
_bl(2) = 63800
_bl(3) = 63800
_yaw_kp_err = 0
_yaw_ki_sum = 0
_yaw_kd_err = 0
_yaw_ki_err = 0
_yaw_kd_old = 0
_roll_kp_err = 0
_roll_ki_sum = 0
_roll_kd_err = 0
_roll_ki_err = 0
_roll_kd_old = 0
_pitch_kp_err = 0
_pitch_ki_sum = 0
_pitch_kd_err = 0
_pitch_ki_err = 0
_pitch_kd_old = 0
_yaw_pid = 0
_roll_pid = 0
_pitch_pid = 0
End If
_sbl_old = _sbl(5)
End Sub
Sub Send_data()
_crc = Crc16(_bl(1) , 4)
Printbin Start_byte ; _bl(1) ; _bl(2) ; _bl(3) ; _bl(4) ; _crc
End Sub
Sub Init_system()
_yawnow = 0
_rollnow = 0
_pitchnow = 0
Call Wmp_init()
Wait 1
For I = 1 To 50 'the first measurements are trash!
Call Read_wmp_data()
Next I
Wait 1
Call Set_wmp_offset()
End Sub
End
Ich habe schon lange gesucht, aber keinen Fehler gefunden! Der Code ist noch nicht geschönt und optimiert, aber er funktioniert gut.
Vielleicht weiß ja jemand von euch, wie ich den Fehler finden köntte oder ihm fällt jetzt schon was auf.. Ich wäre sehr dankbar, da mich das doch sehr stört.
Vielen Dank & Gruß
Chris
EDIT:
Weiß jemand zufällig, wie man das Sinken des Copters bei zu starker Schrägstellung einwenig kompensieren kann? Ich denke, das müsste doch irgendwie mittels der I-Anteile der 2 Regler (Pitch und Roll) funktionieren?! Sollte ich es mal probieren, diese jeweils in positiver Form auf alle Motoren zu geben? Wie würdet ihr soetwas machen? Oder ist es üblich, dass der Pilot das durch mehr Gas kompensiert?