Che Guevara
04.05.2012, 08:31
Hallo,
mein Quadrocopter, ausgestattet mit einem XMega32A4 und MPU6000, 4x Roxxy BL2827-34 Motoren und 18A Graupner BL-Control fliegt mittlerweile schon sehr gut. Leider passiert sporadisch immer mal wieder ein Fehler bei Roll-Loops. Wenn ich um die Roll-Achse drehen möchte (360°) passiert es manchmal, dass eine Achse komplett zu spinnen beginnt. Das Ergebnis, der Quadro fällt ca. 20m taumelnd vom Himmel und ist teilweise geschrottet. Loopings um die Nick-Achse sind dagegen überhaupt kein Problem! Auch im normalen Flug passiert so etwas nicht, wirklich nur manchmal bei Roll-Loopings. Allerdings kann ich mir dieses Verhalten nicht erklären. Es scheint, als ob der rechte vordere und der linke hintere Motor (oder vorne links und hinten rechts) nicht mehr richtig reagieren. Dieses Verhalten zeigt sich sowohl mit der Shrediquette-Software von Willa als auch mit meiner eigenen abgespeckten Software.
Ich hatte schon überlegt, ob es an den Reglern liegt, aber das wiederspricht sich dadurch, dass ich Nick-Flips problemlos machen kann. Vielleicht schaut jemand mal vorsichtshalber über meinen Code!? Oder fällt euch sonst was ein?
$regfile = "xm32a4def.dat"
$framesize = 100
$swstack = 100
$hwstack = 100
$crystal = 32000000
$lib "xmega.lib"
$external _xmegafix_clear
$external _xmegafix_rol_r1014
Config Osc = Disabled , 32mhzosc = Enabled
Config Sysclock = 32mhz , Prescalea = 1 , Prescalebc = 1_1
Declare Sub Rcsignal()
Declare Sub Pidregulator()
Declare Sub Drivemotors()
Declare Sub Init_mpu()
Declare Sub Read_gyro()
Declare Sub Read_acc()
'--RC Settings--
Config Pinc.2 = Input
On Portc_int0 Getreceiver
Enable Portc_int0 , Lo
Portc_pin2ctrl = &B00_011_001
Portc_int0mask = &B0000_0100
Portc_intctrl = &B0000_00_01
Config Tcc0 = Normal , Prescale = 256 , Resolution = 16
On Tcc0_ovf Detectrxpause
Enable Tcc0_ovf , Lo
'--UART Settings--
Config Com2 = 38400 , Mode = Asynchroneous , Parity = None , Stopbits = 1 , Databits = 8
Config Serialin1 = Buffered , Size = 254
Config Serialout1 = Buffered , Size = 254
Open "COM2:" For Binary As #1
'--TWI Settings--
Dim Twi_start As Byte
Open "twie" For Binary As #2
Config Twie = 400000
I2cinit #2
'--Motor Constants--
Const Pwm_mot_off = 20000
Const Pwm_mot_max = 60000
'--PWM Settings--
Config Tcd0 = Pwm , Prescale = 1 , Comparea = Enabled , Compareb = Enabled , Comparec = Enabled , Compared = Enabled , Event_source = Off , Event_action = Off , Event_delay = Disabled , Resolution = 16
Tcd0_cca = Pwm_mot_off
Tcd0_ccb = Pwm_mot_off
Tcd0_ccc = Pwm_mot_off
Tcd0_ccd = Pwm_mot_off
'--LED--
Config Portb.0 = Output
Config Portb.1 = Output
Config Portb.2 = Output
Led1 Alias Portb.0
Led2 Alias Portb.1
Led3 Alias Portb.2
Led1 = 0
Led2 = 0
Led3 = 0
'--MPU 60X0--
Const Mpuaddw = &B11010000
Const Mpuaddr = &B11010001
Dim Gyrox As Integer
Dim Gyroy As Integer
Dim Gyroz As Integer
Dim Tmp_gyrox(2) As Byte At Gyrox Overlay
Dim Tmp_gyroy(2) As Byte At Gyroy Overlay
Dim Tmp_gyroz(2) As Byte At Gyroz Overlay
Dim Accx As Integer
Dim Accy As Integer
Dim Tmp_accx(2) As Byte At Accx Overlay
Dim Tmp_accy(2) As Byte At Accy Overlay
Dim Gyrox_offset As Integer
Dim Gyroy_offset As Integer
Dim Gyroz_offset As Integer
Dim Accx_offset As Integer
Dim Accy_offset As Integer
Dim Roll_check(50) As Integer
Dim Nick_check(50) As Integer
Dim Yaw_check(50) As Integer
Dim Check_tmp As Integer
Dim Offset_tmp As Long
Dim Movement As Byte
Dim Compare_tmp As Integer
'--RC Channel--
Const Throttlechannel = 1
Const Nickchannel = 3
Const Rollchannel = 2
Const Yawchannel = 4
Const Statechannel = 5
'--Read Receiver (Rx)--
Const Maxrcchannel = 8
Dim Empftmp(maxrcchannel) As Word
Dim Empf(maxrcchannel) As Word
Dim Channel As Byte
Dim Lpempf(maxrcchannel) As Word
Dim Intempf(maxrcchannel) As Integer
Dim Receiver_check As Byte
Dim Rc_set_throttle As Long
Dim Rc_set_roll As Long
Dim Rc_set_nick As Long
Dim Rc_set_yaw As Long
Dim Acro_sensitivy As Word
Dim Hover_sensitivy As Word
Dim Yaw_sensitivy As Word
'--Motors--
Dim Vorwahl_pwm As Word
Dim Vorwahl As Word
Dim Motor_rear_left As Long
Dim Motor_rear_right As Long
Dim Motor_front_right As Long
Dim Motor_front_left As Long
'--Control Loop (PID)--
Dim Acro_p As Word
Dim Acro_i As Word
Dim Acro_d As Word
Dim Hover_p As Word
Dim Hover_i As Word
Dim Hover_d As Word
Dim Yaw_p As Word
Dim Yaw_i As Word
Dim Xangle As Single
Dim Yangle As Single
Dim Gyro_yangle As Single
Dim Gyro_xangle As Single
Dim Gyro_influence As Single
Dim Acc_influence As Single
Dim Roll_error As Long
Dim Nick_error As Long
Dim Yaw_error As Long
Dim Roll_error_integral As Long
Dim Nick_error_integral As Long
Dim Yaw_error_integral As Long
Dim Roll_error_differential As Long
Dim Nick_error_differential As Long
Dim Roll_error_old As Long
Dim Nick_error_old As Long
Dim P_set_roll As Long
Dim I_set_roll As Long
Dim D_set_roll As Long
Dim P_set_nick As Long
Dim I_set_nick As Long
Dim D_set_nick As Long
Dim P_set_yaw As Long
Dim I_set_yaw As Long
'-- Internal Variables --
Dim Failsave As Byte
Dim State As Byte
Dim I As Byte
'--Startup--
Vorwahl = 5000
Failsave = 0
'8 Zoll Props:
Acro_p = 270 'Acro_p = 270 / 300
Acro_i = 62 'Acro_i = 62 / 70
Acro_d = 20 'Acro_d = 20 / 25
Acro_sensitivy = 320
Yaw_p = 120 '160
Yaw_i = 29 '40
Hover_p = 40
Hover_i = 25
Hover_d = 140
State = 0
Hover_sensitivy = 100
Yaw_sensitivy = 400
Gyro_influence = 0.99
Acc_influence = 1 - Gyro_influence
Lpempf(1) = 24
Lpempf(2) = 100
Lpempf(3) = 100
Lpempf(4) = 100
Lpempf(5) = 24
Lpempf(6) = 24
Lpempf(7) = 24
Lpempf(8) = 24
Waitms 250
Call Init_mpu()
Waitms 250
Clear Serialin1
Clear Serialout1
'--Get Offset--
For I = 1 To 10
Toggle Led1
Waitms 100
Next I
Led1 = 0
Gyrox_offset = 0
Gyroy_offset = 0
Gyroz_offset = 0
Movement = 1
While Movement = 1
Movement = 0
For I = 1 To 50
Call Read_gyro()
Roll_check(i) = Gyrox
Nick_check(i) = Gyroy
Yaw_check(i) = Gyroz
Waitms 10
Next I
Offset_tmp = 0
For I = 1 To 50
Offset_tmp = Offset_tmp + Roll_check(i)
Next I
Offset_tmp = Offset_tmp / 50
For I = 1 To 50
Compare_tmp = Offset_tmp - Roll_check(i)
Compare_tmp = Abs(compare_tmp)
If Compare_tmp >= 64 Then Movement = 1
Next I
Offset_tmp = 0
For I = 1 To 50
Offset_tmp = Offset_tmp + Nick_check(i)
Next I
Offset_tmp = Offset_tmp / 50
For I = 1 To 50
Compare_tmp = Offset_tmp - Nick_check(i)
Compare_tmp = Abs(compare_tmp)
If Compare_tmp >= 64 Then Movement = 1
Next I
Offset_tmp = 0
For I = 1 To 50
Offset_tmp = Offset_tmp + Yaw_check(i)
Next I
Offset_tmp = Offset_tmp / 50
For I = 1 To 50
Compare_tmp = Offset_tmp - Yaw_check(i)
Compare_tmp = Abs(compare_tmp)
If Compare_tmp >= 64 Then Movement = 1
Next I
Toggle Led1
Wend
Led1 = 0
For I = 1 To 10
Toggle Led2
Waitms 100
Next I
Led2 = 0
Offset_tmp = 0
For I = 1 To 50
Offset_tmp = Offset_tmp + Roll_check(i)
Next I
Offset_tmp = Offset_tmp / 50
Gyrox_offset = Offset_tmp
Offset_tmp = 0
For I = 1 To 50
Offset_tmp = Offset_tmp + Nick_check(i)
Next I
Offset_tmp = Offset_tmp / 50
Gyroy_offset = Offset_tmp
Offset_tmp = 0
For I = 1 To 50
Offset_tmp = Offset_tmp + Yaw_check(i)
Next I
Offset_tmp = Offset_tmp / 50
Gyroz_offset = Offset_tmp
'--Enable Interrupts--
Config Priority = Static , Vector = Application , Lo = Enabled , Med = Enabled , Hi = Enabled
Enable Interrupts
Waitms 100
'check for receiver
Receiver_check = 0
While Receiver_check < 200
If Empf(statechannel) > 50 Or Empf(statechannel) < 20 Or Empf(throttlechannel) > 50 Or Empf(throttlechannel) < 20 Then
If Receiver_check > 0 Then Decr Receiver_check
Else
If Receiver_check < 250 Then Incr Receiver_check
End If
Wend
Do
Call Rcsignal()
Call Pidregulator()
Call Drivemotors()
Loop
End
Sub Rcsignal()
For I = 1 To 4
Lpempf(i) = Lpempf(i) * 3
Lpempf(i) = Lpempf(i) + Empf(i)
Shift Lpempf(i) , Right , 2 , Signed
If Lpempf(i) < 22 Then Lpempf(i) = 22
If Lpempf(i) >= 179 Then Lpempf(i) = 178
Next I
For I = 5 To Maxrcchannel
Lpempf(i) = Lpempf(i) * 7
Lpempf(i) = Lpempf(i) + Empf(i)
Shift Lpempf(i) , Right , 3 , Signed
If Lpempf(i) < 22 Then Lpempf(i) = 22
If Lpempf(i) >= 179 Then Lpempf(i) = 178
Next I
Intempf(1) = Lpempf(throttlechannel) - 22
For I = 2 To Maxrcchannel
Intempf(i) = Lpempf(i) - 100
Next I
If Channel >= 12 Then
If Failsave < 250 Then Incr Failsave
Else
If Failsave >= 1 Then Decr Failsave
End If
If State = 0 Then
If Intempf(throttlechannel) < 20 Then
If Intempf(statechannel) >= -30 And Intempf(statechannel) < 30 Then
State = 1
End If
End If
Else
If Intempf(statechannel) < -30 Then
If Intempf(throttlechannel) < 20 Then
State = 0
End If
Elseif Intempf(statechannel) >= -30 And Intempf(statechannel) < 30 Then
State = 1
Elseif Intempf(statechannel) >= 30 Then
State = 2
End If
End If
Led1 = 0
Led2 = 0
Led3 = 0
If State = 0 Then Led1 = 1
If State = 1 Then Led2 = 1
If State = 2 Then Led3 = 1
Rc_set_throttle = Intempf(throttlechannel) * 250
Rc_set_yaw = Intempf(yawchannel) * Yaw_sensitivy
End Sub
Sub Pidregulator()
Call Read_gyro()
If State = 1 Then
Rc_set_roll = Intempf(rollchannel) * Acro_sensitivy
Rc_set_nick = Intempf(nickchannel) * Acro_sensitivy
Roll_error = Gyrox
Roll_error = Roll_error - Rc_set_roll
Roll_error_integral = Roll_error_integral + Roll_error
If Roll_error_integral >= 320000 Then Roll_error_integral = 320000
If Roll_error_integral < -320000 Then Roll_error_integral = -320000
Roll_error_differential = Roll_error - Roll_error_old
Roll_error_old = Roll_error
P_set_roll = Roll_error * Acro_p
Shift P_set_roll , Right , 9 , Signed 'P_set_roll = P_set_roll / 512
I_set_roll = Roll_error_integral * Acro_i
Shift I_set_roll , Right , 14 , Signed 'I_set_roll = I_set_roll / 16384
D_set_roll = Roll_error_differential * Acro_d
Shift D_set_roll , Right , 9 , Signed 'D_set_roll = D_set_roll / 512
Nick_error = Gyroy
Nick_error = Nick_error - Rc_set_nick
Nick_error_integral = Nick_error_integral + Nick_error
If Nick_error_integral >= 320000 Then Nick_error_integral = 320000
If Nick_error_integral < -320000 Then Nick_error_integral = -320000
Nick_error_differential = Nick_error - Nick_error_old
Nick_error_old = Nick_error
P_set_nick = Nick_error * Acro_p
Shift P_set_nick , Right , 9 , Signed 'P_set_nick = P_set_nick / 512
I_set_nick = Nick_error_integral * Acro_i
Shift I_set_nick , Right , 14 , Signed 'I_set_nick = I_set_Nick / 16384
D_set_nick = Nick_error_differential * Acro_d
Shift D_set_nick , Right , 9 , Signed 'D_set_nick = D_set_nick / 512
Elseif State = 2 Then
Call Read_acc()
Rc_set_roll = Intempf(rollchannel) * Hover_sensitivy
Rc_set_nick = Intempf(nickchannel) * Hover_sensitivy
Gyro_yangle = Gyro_yangle + Gyrox
Gyro_yangle = Gyro_yangle * Gyro_influence
Yangle = Accy * Acc_influence
Gyro_yangle = Gyro_yangle + Yangle
Roll_error = Gyro_yangle - Rc_set_roll
Roll_error_integral = Roll_error_integral + Roll_error
If Roll_error_integral >= 4000000 Then Roll_error_integral = 4000000
If Roll_error_integral < -4000000 Then Roll_error_integral = -4000000
P_set_roll = Roll_error * Hover_p
Shift P_set_roll , Right , 8 , Signed 'P_set_roll = P_set_roll / 256
I_set_roll = Roll_error_integral * Hover_i
Shift I_set_roll , Right , 10 , Signed 'I_set_roll = I_set_roll / 1024
D_set_roll = Gyrox * Hover_d
Shift D_set_roll , Right , 4 , Signed 'D_set_roll = D_set_roll / 16
Gyro_xangle = Gyro_xangle + Gyroy
Gyro_xangle = Gyro_xangle * Gyro_influence
Xangle = Accx * Acc_influence
Gyro_xangle = Gyro_xangle + Xangle
Nick_error = Gyro_xangle - Rc_set_nick
Nick_error_integral = Nick_error_integral + Nick_error
If Nick_error_integral >= 4000000 Then Nick_error_integral = 4000000
If Nick_error_integral < -4000000 Then Nick_error_integral = -4000000
P_set_nick = Nick_error * Hover_p
Shift P_set_nick , Right , 8 , Signed 'P_set_nick = P_set_nick / 256
I_set_nick = Nick_error_integral * Hover_i
Shift I_set_nick , Right , 10 , Signed 'I_set_nick = I_set_nick / 1024
D_set_nick = Gyroy * Hover_d
Shift D_set_nick , Right , 4 , Signed 'D_Set_nick = D_set_nick / 16
Elseif State = 0 Then
Roll_error_integral = 0
Nick_error_integral = 0
Roll_error_old = 0
Nick_error_old = 0
End If
If State >= 1 Then
Yaw_error = Gyroz
Yaw_error = Rc_set_yaw - Yaw_error
Yaw_error_integral = Yaw_error_integral + Yaw_error
If Yaw_error_integral >= 1024000 Then Yaw_error_integral = 1024000
If Yaw_error_integral < -1024000 Then Yaw_error_integral = -1024000
P_set_yaw = Yaw_error * Yaw_p
Shift P_set_yaw , Right , 9 , Signed 'P_set_yaw = P_set_yaw / 512
I_set_yaw = Yaw_error_integral * Yaw_i
Shift I_set_yaw , Right , 14 , Signed 'I_set_yaw = I_set_yaw / 16384
Elseif State = 0 Then
Yaw_error_integral = 0
End If
End Sub
Sub Drivemotors()
Vorwahl_pwm = Vorwahl + Pwm_mot_off
If State >= 1 Then
'--- Motor Rear Left ---
Motor_rear_left = Pwm_mot_off + Rc_set_throttle
Motor_rear_left = Motor_rear_left + Vorwahl
If Motor_rear_left > 55000 Then Motor_rear_left = 55000
Motor_rear_left = Motor_rear_left + P_set_roll
Motor_rear_left = Motor_rear_left + I_set_roll
Motor_rear_left = Motor_rear_left + D_set_roll
Motor_rear_left = Motor_rear_left - P_set_nick
Motor_rear_left = Motor_rear_left - I_set_nick
Motor_rear_left = Motor_rear_left - D_set_nick
Motor_rear_left = Motor_rear_left - P_set_yaw
Motor_rear_left = Motor_rear_left - I_set_yaw
If Motor_rear_left >= Pwm_mot_max Then Motor_rear_left = Pwm_mot_max
If Motor_rear_left < Vorwahl_pwm Then Motor_rear_left = Vorwahl_pwm
'--- Motor Rear Right ---
Motor_rear_right = Pwm_mot_off + Rc_set_throttle
Motor_rear_right = Motor_rear_right + Vorwahl
If Motor_rear_right >= 55000 Then Motor_rear_right = 55000
Motor_rear_right = Motor_rear_right - P_set_roll
Motor_rear_right = Motor_rear_right - I_set_roll
Motor_rear_right = Motor_rear_right - D_set_roll
Motor_rear_right = Motor_rear_right - P_set_nick
Motor_rear_right = Motor_rear_right - I_set_nick
Motor_rear_right = Motor_rear_right - D_set_nick
Motor_rear_right = Motor_rear_right + P_set_yaw
Motor_rear_right = Motor_rear_right + I_set_yaw
If Motor_rear_right >= Pwm_mot_max Then Motor_rear_right = Pwm_mot_max
If Motor_rear_right < Vorwahl_pwm Then Motor_rear_right = Vorwahl_pwm
'--- Motor Front Right ---
Motor_front_right = Pwm_mot_off + Rc_set_throttle
Motor_front_right = Motor_front_right + Vorwahl
If Motor_front_right >= 55000 Then Motor_front_right = 55000
Motor_front_right = Motor_front_right - P_set_roll
Motor_front_right = Motor_front_right - I_set_roll
Motor_front_right = Motor_front_right - D_set_roll
Motor_front_right = Motor_front_right + P_set_nick
Motor_front_right = Motor_front_right + I_set_nick
Motor_front_right = Motor_front_right + D_set_nick
Motor_front_right = Motor_front_right - P_set_yaw
Motor_front_right = Motor_front_right - I_set_yaw
If Motor_front_right >= Pwm_mot_max Then Motor_front_right = Pwm_mot_max
If Motor_front_right < Vorwahl_pwm Then Motor_front_right = Vorwahl_pwm
'--- Motor Front Left ---
Motor_front_left = Pwm_mot_off + Rc_set_throttle
Motor_front_left = Motor_front_left + Vorwahl
If Motor_front_left >= 55000 Then Motor_front_left = 550000
Motor_front_left = Motor_front_left + P_set_roll
Motor_front_left = Motor_front_left + I_set_roll
Motor_front_left = Motor_front_left + D_set_roll
Motor_front_left = Motor_front_left + P_set_nick
Motor_front_left = Motor_front_left + I_set_nick
Motor_front_left = Motor_front_left + D_set_nick
Motor_front_left = Motor_front_left + P_set_yaw
Motor_front_left = Motor_front_left + I_set_yaw
If Motor_front_left >= Pwm_mot_max Then Motor_front_left = Pwm_mot_max
If Motor_front_left < Vorwahl_pwm Then Motor_front_left = Vorwahl_pwm
Elseif State = 0 Then
Motor_rear_left = Pwm_mot_off
Motor_rear_right = Pwm_mot_off
Motor_front_right = Pwm_mot_off
Motor_front_left = Pwm_mot_off
End If
If Failsave < 15 Then
Tcd0_cca = Motor_front_left
Tcd0_ccb = Motor_front_right
Tcd0_ccc = Motor_rear_right
Tcd0_ccd = Motor_rear_left
Else
Tcd0_cca = Pwm_mot_off
Tcd0_ccb = Pwm_mot_off
Tcd0_ccc = Pwm_mot_off
Tcd0_ccd = Pwm_mot_off
End If
End Sub
Sub Init_mpu()
I2cstart #2 'start condition
I2cwbyte Mpuaddw , #2 'write adress of MPU-6050
I2cwbyte 25 , #2 'Register 25 Sample Rate Divider (1..8 kHz)
I2cwbyte &B00000000 , #2 'Divider set to 1 (soll)
I2cstop #2 'stop condition
I2cstart #2 'start condition
I2cwbyte Mpuaddw , #2 'write adress of MPU-6050
I2cwbyte 26 , #2 'Register 26 DLPF_CFG (digital lowpass filter) Configuration
I2cwbyte &B00000000 , #2 'Bits 0..2 = 000 - ACC:260Hz, 0.0ms; Gyro:256Hz, 0.98ms ( &B00000000 )
I2cstop #2 'stop condition
I2cstart #2 'start condition
I2cwbyte Mpuaddw , #2 'write adress of MPU-6050
I2cwbyte 27 , #2 'Register 27 Gyro Configuration
I2cwbyte &B00001000 , #2 'Bits 3+4 = 00 - Full Scale Range: +/-250°/s
I2cstop #2 'stop condition
I2cstart #2 'start condition
I2cwbyte Mpuaddw , #2 'write adress of MPU-6050
I2cwbyte 28 , #2 'Register 28 ACC Configuration
I2cwbyte &B00000000 , #2 'Bits 3+4 = 00 - Full Scale Range: +/-2g / No High Pass Filter
I2cstop #2 'stop condition
I2cstart #2 'start condition
I2cwbyte Mpuaddw , #2 'write adress of MPU-6050
I2cwbyte 107 , #2 'Register 107 Power Management 1
I2cwbyte &B00001011 , #2 'No Reset / No Sleep / No Cycle / Temp_Sens: Dis / Clock Source: Z-Gyro
I2cstop #2
End Sub
Sub Read_gyro()
I2cstart #2
I2cwbyte Mpuaddw , #2
I2cwbyte 67 , #2
I2crepstart #2
I2cwbyte Mpuaddr , #2
I2crbyte Tmp_gyrox(2) , Ack , #2
I2crbyte Tmp_gyrox(1) , Ack , #2
I2crbyte Tmp_gyroy(2) , Ack , #2
I2crbyte Tmp_gyroy(1) , Ack , #2
I2crbyte Tmp_gyroz(2) , Ack , #2
I2crbyte Tmp_gyroz(1) , Nack , #2
I2cstop #2
Gyrox = 0 - Gyrox
'(
Teile Gyro durch 32
Shift Gyrox , Right , 5 , Signed
Shift Gyroy , Right , 5 , Signed
Shift Gyroz , Right , 5 , Signed
')
Gyrox = Gyrox - Gyrox_offset
Gyroy = Gyroy - Gyroy_offset
Gyroz = Gyroz - Gyroz_offset
End Sub
Sub Read_acc()
I2cstart #2
I2cwbyte Mpuaddw , #2
I2cwbyte 59 , #2
I2crepstart #2
I2cwbyte Mpuaddr , #2
I2crbyte Tmp_accx(2) , Ack , #2
I2crbyte Tmp_accx(1) , Ack , #2
I2crbyte Tmp_accy(2) , Ack , #2
I2crbyte Tmp_accy(1) , Nack , #2
I2cstop #2
Accx = -accx
Accy = -accy
Shift Accx , Left , 3 , Signed
Shift Accy , Left , 3 , Signed
Accx = Accx - Accx_offset
Accy = Accy - Accy_offset
End Sub
Getreceiver:
If Channel >= 1 And Channel <= Maxrcchannel Then
Empftmp(channel) = Tcc0_cnt - 65092
Empf(channel) = Empftmp(channel)
End If
Tcc0_cnt = 65000
Incr Channel
Return
Detectrxpause:
Channel = 0
Return
Wenn nötig, könnte ich das Verhalten mal filmen, dass ihr euch ein besseres Bild davon machen könntet.
Gruß
Chris
mein Quadrocopter, ausgestattet mit einem XMega32A4 und MPU6000, 4x Roxxy BL2827-34 Motoren und 18A Graupner BL-Control fliegt mittlerweile schon sehr gut. Leider passiert sporadisch immer mal wieder ein Fehler bei Roll-Loops. Wenn ich um die Roll-Achse drehen möchte (360°) passiert es manchmal, dass eine Achse komplett zu spinnen beginnt. Das Ergebnis, der Quadro fällt ca. 20m taumelnd vom Himmel und ist teilweise geschrottet. Loopings um die Nick-Achse sind dagegen überhaupt kein Problem! Auch im normalen Flug passiert so etwas nicht, wirklich nur manchmal bei Roll-Loopings. Allerdings kann ich mir dieses Verhalten nicht erklären. Es scheint, als ob der rechte vordere und der linke hintere Motor (oder vorne links und hinten rechts) nicht mehr richtig reagieren. Dieses Verhalten zeigt sich sowohl mit der Shrediquette-Software von Willa als auch mit meiner eigenen abgespeckten Software.
Ich hatte schon überlegt, ob es an den Reglern liegt, aber das wiederspricht sich dadurch, dass ich Nick-Flips problemlos machen kann. Vielleicht schaut jemand mal vorsichtshalber über meinen Code!? Oder fällt euch sonst was ein?
$regfile = "xm32a4def.dat"
$framesize = 100
$swstack = 100
$hwstack = 100
$crystal = 32000000
$lib "xmega.lib"
$external _xmegafix_clear
$external _xmegafix_rol_r1014
Config Osc = Disabled , 32mhzosc = Enabled
Config Sysclock = 32mhz , Prescalea = 1 , Prescalebc = 1_1
Declare Sub Rcsignal()
Declare Sub Pidregulator()
Declare Sub Drivemotors()
Declare Sub Init_mpu()
Declare Sub Read_gyro()
Declare Sub Read_acc()
'--RC Settings--
Config Pinc.2 = Input
On Portc_int0 Getreceiver
Enable Portc_int0 , Lo
Portc_pin2ctrl = &B00_011_001
Portc_int0mask = &B0000_0100
Portc_intctrl = &B0000_00_01
Config Tcc0 = Normal , Prescale = 256 , Resolution = 16
On Tcc0_ovf Detectrxpause
Enable Tcc0_ovf , Lo
'--UART Settings--
Config Com2 = 38400 , Mode = Asynchroneous , Parity = None , Stopbits = 1 , Databits = 8
Config Serialin1 = Buffered , Size = 254
Config Serialout1 = Buffered , Size = 254
Open "COM2:" For Binary As #1
'--TWI Settings--
Dim Twi_start As Byte
Open "twie" For Binary As #2
Config Twie = 400000
I2cinit #2
'--Motor Constants--
Const Pwm_mot_off = 20000
Const Pwm_mot_max = 60000
'--PWM Settings--
Config Tcd0 = Pwm , Prescale = 1 , Comparea = Enabled , Compareb = Enabled , Comparec = Enabled , Compared = Enabled , Event_source = Off , Event_action = Off , Event_delay = Disabled , Resolution = 16
Tcd0_cca = Pwm_mot_off
Tcd0_ccb = Pwm_mot_off
Tcd0_ccc = Pwm_mot_off
Tcd0_ccd = Pwm_mot_off
'--LED--
Config Portb.0 = Output
Config Portb.1 = Output
Config Portb.2 = Output
Led1 Alias Portb.0
Led2 Alias Portb.1
Led3 Alias Portb.2
Led1 = 0
Led2 = 0
Led3 = 0
'--MPU 60X0--
Const Mpuaddw = &B11010000
Const Mpuaddr = &B11010001
Dim Gyrox As Integer
Dim Gyroy As Integer
Dim Gyroz As Integer
Dim Tmp_gyrox(2) As Byte At Gyrox Overlay
Dim Tmp_gyroy(2) As Byte At Gyroy Overlay
Dim Tmp_gyroz(2) As Byte At Gyroz Overlay
Dim Accx As Integer
Dim Accy As Integer
Dim Tmp_accx(2) As Byte At Accx Overlay
Dim Tmp_accy(2) As Byte At Accy Overlay
Dim Gyrox_offset As Integer
Dim Gyroy_offset As Integer
Dim Gyroz_offset As Integer
Dim Accx_offset As Integer
Dim Accy_offset As Integer
Dim Roll_check(50) As Integer
Dim Nick_check(50) As Integer
Dim Yaw_check(50) As Integer
Dim Check_tmp As Integer
Dim Offset_tmp As Long
Dim Movement As Byte
Dim Compare_tmp As Integer
'--RC Channel--
Const Throttlechannel = 1
Const Nickchannel = 3
Const Rollchannel = 2
Const Yawchannel = 4
Const Statechannel = 5
'--Read Receiver (Rx)--
Const Maxrcchannel = 8
Dim Empftmp(maxrcchannel) As Word
Dim Empf(maxrcchannel) As Word
Dim Channel As Byte
Dim Lpempf(maxrcchannel) As Word
Dim Intempf(maxrcchannel) As Integer
Dim Receiver_check As Byte
Dim Rc_set_throttle As Long
Dim Rc_set_roll As Long
Dim Rc_set_nick As Long
Dim Rc_set_yaw As Long
Dim Acro_sensitivy As Word
Dim Hover_sensitivy As Word
Dim Yaw_sensitivy As Word
'--Motors--
Dim Vorwahl_pwm As Word
Dim Vorwahl As Word
Dim Motor_rear_left As Long
Dim Motor_rear_right As Long
Dim Motor_front_right As Long
Dim Motor_front_left As Long
'--Control Loop (PID)--
Dim Acro_p As Word
Dim Acro_i As Word
Dim Acro_d As Word
Dim Hover_p As Word
Dim Hover_i As Word
Dim Hover_d As Word
Dim Yaw_p As Word
Dim Yaw_i As Word
Dim Xangle As Single
Dim Yangle As Single
Dim Gyro_yangle As Single
Dim Gyro_xangle As Single
Dim Gyro_influence As Single
Dim Acc_influence As Single
Dim Roll_error As Long
Dim Nick_error As Long
Dim Yaw_error As Long
Dim Roll_error_integral As Long
Dim Nick_error_integral As Long
Dim Yaw_error_integral As Long
Dim Roll_error_differential As Long
Dim Nick_error_differential As Long
Dim Roll_error_old As Long
Dim Nick_error_old As Long
Dim P_set_roll As Long
Dim I_set_roll As Long
Dim D_set_roll As Long
Dim P_set_nick As Long
Dim I_set_nick As Long
Dim D_set_nick As Long
Dim P_set_yaw As Long
Dim I_set_yaw As Long
'-- Internal Variables --
Dim Failsave As Byte
Dim State As Byte
Dim I As Byte
'--Startup--
Vorwahl = 5000
Failsave = 0
'8 Zoll Props:
Acro_p = 270 'Acro_p = 270 / 300
Acro_i = 62 'Acro_i = 62 / 70
Acro_d = 20 'Acro_d = 20 / 25
Acro_sensitivy = 320
Yaw_p = 120 '160
Yaw_i = 29 '40
Hover_p = 40
Hover_i = 25
Hover_d = 140
State = 0
Hover_sensitivy = 100
Yaw_sensitivy = 400
Gyro_influence = 0.99
Acc_influence = 1 - Gyro_influence
Lpempf(1) = 24
Lpempf(2) = 100
Lpempf(3) = 100
Lpempf(4) = 100
Lpempf(5) = 24
Lpempf(6) = 24
Lpempf(7) = 24
Lpempf(8) = 24
Waitms 250
Call Init_mpu()
Waitms 250
Clear Serialin1
Clear Serialout1
'--Get Offset--
For I = 1 To 10
Toggle Led1
Waitms 100
Next I
Led1 = 0
Gyrox_offset = 0
Gyroy_offset = 0
Gyroz_offset = 0
Movement = 1
While Movement = 1
Movement = 0
For I = 1 To 50
Call Read_gyro()
Roll_check(i) = Gyrox
Nick_check(i) = Gyroy
Yaw_check(i) = Gyroz
Waitms 10
Next I
Offset_tmp = 0
For I = 1 To 50
Offset_tmp = Offset_tmp + Roll_check(i)
Next I
Offset_tmp = Offset_tmp / 50
For I = 1 To 50
Compare_tmp = Offset_tmp - Roll_check(i)
Compare_tmp = Abs(compare_tmp)
If Compare_tmp >= 64 Then Movement = 1
Next I
Offset_tmp = 0
For I = 1 To 50
Offset_tmp = Offset_tmp + Nick_check(i)
Next I
Offset_tmp = Offset_tmp / 50
For I = 1 To 50
Compare_tmp = Offset_tmp - Nick_check(i)
Compare_tmp = Abs(compare_tmp)
If Compare_tmp >= 64 Then Movement = 1
Next I
Offset_tmp = 0
For I = 1 To 50
Offset_tmp = Offset_tmp + Yaw_check(i)
Next I
Offset_tmp = Offset_tmp / 50
For I = 1 To 50
Compare_tmp = Offset_tmp - Yaw_check(i)
Compare_tmp = Abs(compare_tmp)
If Compare_tmp >= 64 Then Movement = 1
Next I
Toggle Led1
Wend
Led1 = 0
For I = 1 To 10
Toggle Led2
Waitms 100
Next I
Led2 = 0
Offset_tmp = 0
For I = 1 To 50
Offset_tmp = Offset_tmp + Roll_check(i)
Next I
Offset_tmp = Offset_tmp / 50
Gyrox_offset = Offset_tmp
Offset_tmp = 0
For I = 1 To 50
Offset_tmp = Offset_tmp + Nick_check(i)
Next I
Offset_tmp = Offset_tmp / 50
Gyroy_offset = Offset_tmp
Offset_tmp = 0
For I = 1 To 50
Offset_tmp = Offset_tmp + Yaw_check(i)
Next I
Offset_tmp = Offset_tmp / 50
Gyroz_offset = Offset_tmp
'--Enable Interrupts--
Config Priority = Static , Vector = Application , Lo = Enabled , Med = Enabled , Hi = Enabled
Enable Interrupts
Waitms 100
'check for receiver
Receiver_check = 0
While Receiver_check < 200
If Empf(statechannel) > 50 Or Empf(statechannel) < 20 Or Empf(throttlechannel) > 50 Or Empf(throttlechannel) < 20 Then
If Receiver_check > 0 Then Decr Receiver_check
Else
If Receiver_check < 250 Then Incr Receiver_check
End If
Wend
Do
Call Rcsignal()
Call Pidregulator()
Call Drivemotors()
Loop
End
Sub Rcsignal()
For I = 1 To 4
Lpempf(i) = Lpempf(i) * 3
Lpempf(i) = Lpempf(i) + Empf(i)
Shift Lpempf(i) , Right , 2 , Signed
If Lpempf(i) < 22 Then Lpempf(i) = 22
If Lpempf(i) >= 179 Then Lpempf(i) = 178
Next I
For I = 5 To Maxrcchannel
Lpempf(i) = Lpempf(i) * 7
Lpempf(i) = Lpempf(i) + Empf(i)
Shift Lpempf(i) , Right , 3 , Signed
If Lpempf(i) < 22 Then Lpempf(i) = 22
If Lpempf(i) >= 179 Then Lpempf(i) = 178
Next I
Intempf(1) = Lpempf(throttlechannel) - 22
For I = 2 To Maxrcchannel
Intempf(i) = Lpempf(i) - 100
Next I
If Channel >= 12 Then
If Failsave < 250 Then Incr Failsave
Else
If Failsave >= 1 Then Decr Failsave
End If
If State = 0 Then
If Intempf(throttlechannel) < 20 Then
If Intempf(statechannel) >= -30 And Intempf(statechannel) < 30 Then
State = 1
End If
End If
Else
If Intempf(statechannel) < -30 Then
If Intempf(throttlechannel) < 20 Then
State = 0
End If
Elseif Intempf(statechannel) >= -30 And Intempf(statechannel) < 30 Then
State = 1
Elseif Intempf(statechannel) >= 30 Then
State = 2
End If
End If
Led1 = 0
Led2 = 0
Led3 = 0
If State = 0 Then Led1 = 1
If State = 1 Then Led2 = 1
If State = 2 Then Led3 = 1
Rc_set_throttle = Intempf(throttlechannel) * 250
Rc_set_yaw = Intempf(yawchannel) * Yaw_sensitivy
End Sub
Sub Pidregulator()
Call Read_gyro()
If State = 1 Then
Rc_set_roll = Intempf(rollchannel) * Acro_sensitivy
Rc_set_nick = Intempf(nickchannel) * Acro_sensitivy
Roll_error = Gyrox
Roll_error = Roll_error - Rc_set_roll
Roll_error_integral = Roll_error_integral + Roll_error
If Roll_error_integral >= 320000 Then Roll_error_integral = 320000
If Roll_error_integral < -320000 Then Roll_error_integral = -320000
Roll_error_differential = Roll_error - Roll_error_old
Roll_error_old = Roll_error
P_set_roll = Roll_error * Acro_p
Shift P_set_roll , Right , 9 , Signed 'P_set_roll = P_set_roll / 512
I_set_roll = Roll_error_integral * Acro_i
Shift I_set_roll , Right , 14 , Signed 'I_set_roll = I_set_roll / 16384
D_set_roll = Roll_error_differential * Acro_d
Shift D_set_roll , Right , 9 , Signed 'D_set_roll = D_set_roll / 512
Nick_error = Gyroy
Nick_error = Nick_error - Rc_set_nick
Nick_error_integral = Nick_error_integral + Nick_error
If Nick_error_integral >= 320000 Then Nick_error_integral = 320000
If Nick_error_integral < -320000 Then Nick_error_integral = -320000
Nick_error_differential = Nick_error - Nick_error_old
Nick_error_old = Nick_error
P_set_nick = Nick_error * Acro_p
Shift P_set_nick , Right , 9 , Signed 'P_set_nick = P_set_nick / 512
I_set_nick = Nick_error_integral * Acro_i
Shift I_set_nick , Right , 14 , Signed 'I_set_nick = I_set_Nick / 16384
D_set_nick = Nick_error_differential * Acro_d
Shift D_set_nick , Right , 9 , Signed 'D_set_nick = D_set_nick / 512
Elseif State = 2 Then
Call Read_acc()
Rc_set_roll = Intempf(rollchannel) * Hover_sensitivy
Rc_set_nick = Intempf(nickchannel) * Hover_sensitivy
Gyro_yangle = Gyro_yangle + Gyrox
Gyro_yangle = Gyro_yangle * Gyro_influence
Yangle = Accy * Acc_influence
Gyro_yangle = Gyro_yangle + Yangle
Roll_error = Gyro_yangle - Rc_set_roll
Roll_error_integral = Roll_error_integral + Roll_error
If Roll_error_integral >= 4000000 Then Roll_error_integral = 4000000
If Roll_error_integral < -4000000 Then Roll_error_integral = -4000000
P_set_roll = Roll_error * Hover_p
Shift P_set_roll , Right , 8 , Signed 'P_set_roll = P_set_roll / 256
I_set_roll = Roll_error_integral * Hover_i
Shift I_set_roll , Right , 10 , Signed 'I_set_roll = I_set_roll / 1024
D_set_roll = Gyrox * Hover_d
Shift D_set_roll , Right , 4 , Signed 'D_set_roll = D_set_roll / 16
Gyro_xangle = Gyro_xangle + Gyroy
Gyro_xangle = Gyro_xangle * Gyro_influence
Xangle = Accx * Acc_influence
Gyro_xangle = Gyro_xangle + Xangle
Nick_error = Gyro_xangle - Rc_set_nick
Nick_error_integral = Nick_error_integral + Nick_error
If Nick_error_integral >= 4000000 Then Nick_error_integral = 4000000
If Nick_error_integral < -4000000 Then Nick_error_integral = -4000000
P_set_nick = Nick_error * Hover_p
Shift P_set_nick , Right , 8 , Signed 'P_set_nick = P_set_nick / 256
I_set_nick = Nick_error_integral * Hover_i
Shift I_set_nick , Right , 10 , Signed 'I_set_nick = I_set_nick / 1024
D_set_nick = Gyroy * Hover_d
Shift D_set_nick , Right , 4 , Signed 'D_Set_nick = D_set_nick / 16
Elseif State = 0 Then
Roll_error_integral = 0
Nick_error_integral = 0
Roll_error_old = 0
Nick_error_old = 0
End If
If State >= 1 Then
Yaw_error = Gyroz
Yaw_error = Rc_set_yaw - Yaw_error
Yaw_error_integral = Yaw_error_integral + Yaw_error
If Yaw_error_integral >= 1024000 Then Yaw_error_integral = 1024000
If Yaw_error_integral < -1024000 Then Yaw_error_integral = -1024000
P_set_yaw = Yaw_error * Yaw_p
Shift P_set_yaw , Right , 9 , Signed 'P_set_yaw = P_set_yaw / 512
I_set_yaw = Yaw_error_integral * Yaw_i
Shift I_set_yaw , Right , 14 , Signed 'I_set_yaw = I_set_yaw / 16384
Elseif State = 0 Then
Yaw_error_integral = 0
End If
End Sub
Sub Drivemotors()
Vorwahl_pwm = Vorwahl + Pwm_mot_off
If State >= 1 Then
'--- Motor Rear Left ---
Motor_rear_left = Pwm_mot_off + Rc_set_throttle
Motor_rear_left = Motor_rear_left + Vorwahl
If Motor_rear_left > 55000 Then Motor_rear_left = 55000
Motor_rear_left = Motor_rear_left + P_set_roll
Motor_rear_left = Motor_rear_left + I_set_roll
Motor_rear_left = Motor_rear_left + D_set_roll
Motor_rear_left = Motor_rear_left - P_set_nick
Motor_rear_left = Motor_rear_left - I_set_nick
Motor_rear_left = Motor_rear_left - D_set_nick
Motor_rear_left = Motor_rear_left - P_set_yaw
Motor_rear_left = Motor_rear_left - I_set_yaw
If Motor_rear_left >= Pwm_mot_max Then Motor_rear_left = Pwm_mot_max
If Motor_rear_left < Vorwahl_pwm Then Motor_rear_left = Vorwahl_pwm
'--- Motor Rear Right ---
Motor_rear_right = Pwm_mot_off + Rc_set_throttle
Motor_rear_right = Motor_rear_right + Vorwahl
If Motor_rear_right >= 55000 Then Motor_rear_right = 55000
Motor_rear_right = Motor_rear_right - P_set_roll
Motor_rear_right = Motor_rear_right - I_set_roll
Motor_rear_right = Motor_rear_right - D_set_roll
Motor_rear_right = Motor_rear_right - P_set_nick
Motor_rear_right = Motor_rear_right - I_set_nick
Motor_rear_right = Motor_rear_right - D_set_nick
Motor_rear_right = Motor_rear_right + P_set_yaw
Motor_rear_right = Motor_rear_right + I_set_yaw
If Motor_rear_right >= Pwm_mot_max Then Motor_rear_right = Pwm_mot_max
If Motor_rear_right < Vorwahl_pwm Then Motor_rear_right = Vorwahl_pwm
'--- Motor Front Right ---
Motor_front_right = Pwm_mot_off + Rc_set_throttle
Motor_front_right = Motor_front_right + Vorwahl
If Motor_front_right >= 55000 Then Motor_front_right = 55000
Motor_front_right = Motor_front_right - P_set_roll
Motor_front_right = Motor_front_right - I_set_roll
Motor_front_right = Motor_front_right - D_set_roll
Motor_front_right = Motor_front_right + P_set_nick
Motor_front_right = Motor_front_right + I_set_nick
Motor_front_right = Motor_front_right + D_set_nick
Motor_front_right = Motor_front_right - P_set_yaw
Motor_front_right = Motor_front_right - I_set_yaw
If Motor_front_right >= Pwm_mot_max Then Motor_front_right = Pwm_mot_max
If Motor_front_right < Vorwahl_pwm Then Motor_front_right = Vorwahl_pwm
'--- Motor Front Left ---
Motor_front_left = Pwm_mot_off + Rc_set_throttle
Motor_front_left = Motor_front_left + Vorwahl
If Motor_front_left >= 55000 Then Motor_front_left = 550000
Motor_front_left = Motor_front_left + P_set_roll
Motor_front_left = Motor_front_left + I_set_roll
Motor_front_left = Motor_front_left + D_set_roll
Motor_front_left = Motor_front_left + P_set_nick
Motor_front_left = Motor_front_left + I_set_nick
Motor_front_left = Motor_front_left + D_set_nick
Motor_front_left = Motor_front_left + P_set_yaw
Motor_front_left = Motor_front_left + I_set_yaw
If Motor_front_left >= Pwm_mot_max Then Motor_front_left = Pwm_mot_max
If Motor_front_left < Vorwahl_pwm Then Motor_front_left = Vorwahl_pwm
Elseif State = 0 Then
Motor_rear_left = Pwm_mot_off
Motor_rear_right = Pwm_mot_off
Motor_front_right = Pwm_mot_off
Motor_front_left = Pwm_mot_off
End If
If Failsave < 15 Then
Tcd0_cca = Motor_front_left
Tcd0_ccb = Motor_front_right
Tcd0_ccc = Motor_rear_right
Tcd0_ccd = Motor_rear_left
Else
Tcd0_cca = Pwm_mot_off
Tcd0_ccb = Pwm_mot_off
Tcd0_ccc = Pwm_mot_off
Tcd0_ccd = Pwm_mot_off
End If
End Sub
Sub Init_mpu()
I2cstart #2 'start condition
I2cwbyte Mpuaddw , #2 'write adress of MPU-6050
I2cwbyte 25 , #2 'Register 25 Sample Rate Divider (1..8 kHz)
I2cwbyte &B00000000 , #2 'Divider set to 1 (soll)
I2cstop #2 'stop condition
I2cstart #2 'start condition
I2cwbyte Mpuaddw , #2 'write adress of MPU-6050
I2cwbyte 26 , #2 'Register 26 DLPF_CFG (digital lowpass filter) Configuration
I2cwbyte &B00000000 , #2 'Bits 0..2 = 000 - ACC:260Hz, 0.0ms; Gyro:256Hz, 0.98ms ( &B00000000 )
I2cstop #2 'stop condition
I2cstart #2 'start condition
I2cwbyte Mpuaddw , #2 'write adress of MPU-6050
I2cwbyte 27 , #2 'Register 27 Gyro Configuration
I2cwbyte &B00001000 , #2 'Bits 3+4 = 00 - Full Scale Range: +/-250°/s
I2cstop #2 'stop condition
I2cstart #2 'start condition
I2cwbyte Mpuaddw , #2 'write adress of MPU-6050
I2cwbyte 28 , #2 'Register 28 ACC Configuration
I2cwbyte &B00000000 , #2 'Bits 3+4 = 00 - Full Scale Range: +/-2g / No High Pass Filter
I2cstop #2 'stop condition
I2cstart #2 'start condition
I2cwbyte Mpuaddw , #2 'write adress of MPU-6050
I2cwbyte 107 , #2 'Register 107 Power Management 1
I2cwbyte &B00001011 , #2 'No Reset / No Sleep / No Cycle / Temp_Sens: Dis / Clock Source: Z-Gyro
I2cstop #2
End Sub
Sub Read_gyro()
I2cstart #2
I2cwbyte Mpuaddw , #2
I2cwbyte 67 , #2
I2crepstart #2
I2cwbyte Mpuaddr , #2
I2crbyte Tmp_gyrox(2) , Ack , #2
I2crbyte Tmp_gyrox(1) , Ack , #2
I2crbyte Tmp_gyroy(2) , Ack , #2
I2crbyte Tmp_gyroy(1) , Ack , #2
I2crbyte Tmp_gyroz(2) , Ack , #2
I2crbyte Tmp_gyroz(1) , Nack , #2
I2cstop #2
Gyrox = 0 - Gyrox
'(
Teile Gyro durch 32
Shift Gyrox , Right , 5 , Signed
Shift Gyroy , Right , 5 , Signed
Shift Gyroz , Right , 5 , Signed
')
Gyrox = Gyrox - Gyrox_offset
Gyroy = Gyroy - Gyroy_offset
Gyroz = Gyroz - Gyroz_offset
End Sub
Sub Read_acc()
I2cstart #2
I2cwbyte Mpuaddw , #2
I2cwbyte 59 , #2
I2crepstart #2
I2cwbyte Mpuaddr , #2
I2crbyte Tmp_accx(2) , Ack , #2
I2crbyte Tmp_accx(1) , Ack , #2
I2crbyte Tmp_accy(2) , Ack , #2
I2crbyte Tmp_accy(1) , Nack , #2
I2cstop #2
Accx = -accx
Accy = -accy
Shift Accx , Left , 3 , Signed
Shift Accy , Left , 3 , Signed
Accx = Accx - Accx_offset
Accy = Accy - Accy_offset
End Sub
Getreceiver:
If Channel >= 1 And Channel <= Maxrcchannel Then
Empftmp(channel) = Tcc0_cnt - 65092
Empf(channel) = Empftmp(channel)
End If
Tcc0_cnt = 65000
Incr Channel
Return
Detectrxpause:
Channel = 0
Return
Wenn nötig, könnte ich das Verhalten mal filmen, dass ihr euch ein besseres Bild davon machen könntet.
Gruß
Chris