Hallo,

um eine Prozedur etwas schneller zu machen, wollte ich versch. Divisionen durch Shiften ersetzen, jedoch habe ich ein Problem:
So wollte ich es machen:
Code:
Shift _long_tmp , Right, 15, Signed
Jedoch scheint das nicht zu funktionieren, warum auch immer?! Die Variable _long_tmp kann sowohl negativ als auch positiv sein. Ist das ein Bug in meiner Version (V1.11.9.5) oder habe ich einen Fehler?
Zum Hintergrund:
Im Code meines Tricopters habe ich einen PID-Regler, welcher mit Integern rechnet, weils schneller als Fließkomma ist. Ich multipliziere den Fehler erst mit der jeweiligen Variable und teile dann durch einen best. Wert... dieses Teilen möchte ich eben durch Shift ersetzen. Damit mir meine Integer Variable nicht überlauft, benutze ich dafür die Variable _long_tmp. Hier mal der komplette Code:
Code:
$regfile = "m328pdef.dat"
$crystal = 16000000
$framesize = 200
$hwstack = 200
$swstack = 200
$baud = 38400


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                                        '0
Config Sda = Portc.4                                        '1
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 = 100                                               '100
_roll_kp = 40                                               '40
_pitch_kp = 85                                              '85

_yaw_ki = 55                                                '55
_roll_ki = 90                                               '90
_pitch_ki = 138                                             '138

_yaw_kd = 0                                                 '0
_roll_kd = 0                                                '120
_pitch_kd = 0                                               '112


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
'#################################
'#################################
'#################################


Wait 2

Call Init_system()

Enable Interrupts



Do


 Call Filter_rx_data()
 Call Filter_gyro_data()
 Call Pid_regulator()
 Call Mixer()
 Call Send_data()


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

 _sbl(2) = _sbl(2) * 10
 _sbl(2) = _sbl(2) / 110

 _sbl(3) = _sbl(3) * 10
 _sbl(3) = _sbl(3) / 110

 _sbl(4) = _sbl(4) * 10
 _sbl(4) = _sbl(4) / 30


 _sbl(7) = _sbl(7) / 10
 _sbl(8) = _sbl(8) / 10

 _roll_kd = 0 + _sbl(7)
 _pitch_kd = 0 + _sbl(8)


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) > -250 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
 Else
  _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

End Sub


Sub Send_data()
  _crc = Crc16(_bl(1) , 4)
  Printbin Start_byte ; _bl(1) ; _bl(2) ; _bl(3) ; _bl(4) ; _crc
  Incr _timer2_counter
  If _timer2_counter = 20 Then
   _timer2_counter = 0
   Print #2 , _roll_kd ; ":" ; _pitch_kd
   'Print #2 , Kanal(1) ; ":" ; Kanal(2) ; ":" ; Kanal(3) ; ":" ; Kanal(4) ; ":" ; Kanal(5) ; ":" ; Kanal(6) ; ":" ; Kanal(7) ; ":" ; Kanal(8)
   'Print #2 , _sbl(1) ; ":" ; _sbl(2) ; ":" ; _sbl(3) ; ":" ; _sbl(4) ; ":" ; _sbl(5) ; ":" ; _sbl(6) ; ":" ; _sbl(7) ; ":" ; _sbl(8)
   'Print #2 , _yaw_kp ; ":" ; _roll_kp ; ":" ; _pitch_kp
   'Print #2 , _yaw_pid ; ":" ; _roll_pid ; ":" ; _pitch_pid
   'Print #2 , "ROLL: " ; _roll_kp_err ; ":" ; _roll_ki_sum ; ":" ; _roll_kd_err ;
   'Print #2 , "PITCH: " ; _pitch_kp_err ; ":" ; _pitch_ki_sum ; ":" ; _pitch_kd_err ;
   'Print #2 , "_______________"
  End If
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
Wenn ichs mit einer normalen Division mache, funktionierts, sprich der Kopter bewegt sich nicht, wenn er still auf dem Tisch steht! Wenn ich aber die Shift-Befehle einbaue, fängt nach ein paar Sekunden des stillen Liegens ein Motor an sich zu drehen! An was kann das liegen?

Gruß
Chris