komisches Verhalten beim Flashen

Hallo,

nachdem ich bei meinem Tricopter gerade dabei bin, die PID-Parameter einzustellen ist mir folgendes aufgefallen:
Wenn ich den µC flashe und dann die Stromversorgung beibehalte und nur den ISP-Stecker trenne, läuft alles wunderbar.
Wenn ich aber den µC flashe und danach die Stromversorgung und den ISP-Stecker trenne und dann den Akku wieder anschließe, drehen sich 2 der 3 Motoren ziemlich schnell, obwohl sich die Wii Motion Plus nicht bewegt...
Kann sich jemand dieses Verhalten erklären?
Zur Info:
Die ISP-Leitungen der beiden µCs (ATTINY2313 & ATMEGA328P) sind miteinander verbunden, lediglich die Reset-Anschlüsse sind jumperbar, damit ich immer den richtigen µC zum flashen auswählen kann. Die WM+ hängt neben Rc-Empfänger am ATMEGA328P, welcher über UART mit dem ATTINY2313 kommuniziert; dieser steuert nur die 4 Aktoren an. Am ATMEGA328P kanns fast nicht liegen, da ich vorher einen ATMEGA8 drauf hatte und dieser zeigte das gleiche Verhalten!
Hier noch der Code:

Code:

---

$regfile = "m328pdef.dat"
$crystal = 16000000
$framesize = 200
$hwstack = 200
$swstack = 200
$baud = 38400


Declare Sub Wmp_init()
Declare Sub Send_zero()
Declare Sub Read_data()
Declare Sub Set_offset()

Declare Sub Filter_gyro_data()
Declare Sub Pid_regulator()
Declare Sub Filter_rx_data()
Declare Sub Mixer()
Declare Sub Send_data()


$lib "I2C_TWI.LBX"                                          'Hardware I2C
Config Scl = Portc.5                                        'Ports for I2C-Bus
Config Sda = Portc.4
Config Twi = 100000
I2cinit


Config Timer1 = Timer , Prescale = 1
On Timer1 Pausedetect
Enable Timer1

Config Int1 = Falling
On Int1 Measure
Enable Int1


Config Pind.3 = Input
Portd.3 = 0


Const Start_byte = 127
Const _maxchannel = 4

Const _bl1offset = 0
Const _bl2offset = 0
Const _bl3offset = 0
Const _bl4offset = 0

Const _yaw_kp = 6
Const _roll_kp = 15
Const _pitch_kp = 15

Const _yaw_ki = 0
Const _roll_ki = 0
Const _pitch_ki = 0

Const _yaw_kd = 0                                          '5
Const _roll_kd = 0
Const _pitch_kd = 0


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


Dim _sbl_filter(4) As Integer
Dim _sbl_filter_2(4) As Integer


Dim Bufferbyte As Byte
Dim Kanal(_maxchannel) As Word
Dim Channel As Byte
Dim _bl(_maxchannel) As Word
Dim I As Byte
Dim _crc As Word
Dim _sbl(_maxchannel) As Integer

Dim Buffer(6) As Byte

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

Dim _empfmiddle(_maxchannel) As Word
Dim _empfmin(_maxchannel) As Word
Dim _empfmax(_maxchannel) As Word
Dim _empfdiv(_maxchannel) As Word
Dim _stick_sensitivy(_maxchannel) As Word

_stick_sensitivy(1) = 2265
_stick_sensitivy(2) = 1800
_stick_sensitivy(3) = 1800
_stick_sensitivy(4) = 1800

_empfmiddle(1) = 26500
_empfmiddle(2) = 23800
_empfmiddle(3) = 25300
_empfmiddle(4) = 22250

_empfmin(1) = 14300
_empfmin(2) = 14650
_empfmin(3) = 17100
_empfmin(4) = 14750

_empfmax(1) = 32300
_empfmax(2) = 32600
_empfmax(3) = 32600
_empfmax(4) = 30500


For I = 1 To 4
 _empfdiv(i) = _empfmiddle(i) - _empfmin(i)
 _empfdiv(i) = _empfdiv(i) / _stick_sensitivy(i)            '2265
 _empfdiv(i) = _empfdiv(i) * 2
Next I


'Wait 1

Call Wmp_init()
Waitms 500
Call Set_offset()
Enable Interrupts


Wait 1



Do


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


Loop


Measure:
If Channel > 0 And Channel < 5 Then
  Kanal(channel) = Timer1
End If
Timer1 = 1536
Incr Channel
Return

Pausedetect:
Channel = 0
Return


Sub Wmp_init()
    I2cstart
    I2cwbyte &HA6                                          ' sends memory address
    I2cwbyte &HFE                                          ' WM+ activation
    I2cwbyte &H04 .                                        ' Now Adress changes to &HA4
    I2cstop
End Sub

Sub Send_zero()
  I2cstart
  I2cwbyte &HA4                                            ' sends memory address
  I2cwbyte &H00                                            ' sends zero before receiving
  I2cstop
  Waitms 1
End Sub

Sub Read_data()
  Gosub Send_zero                                          ' sends zero before receiving
  Gosub Send_zero
  Gosub Send_zero
  I2creceive &HA4 , Buffer(1) , 0 , 6                      ' receive 6 bytes
  Yaw1 = Buffer(1)
  Roll1 = Buffer(2)                                        '  Low Bytes
  Pitch1 = Buffer(3)
  Shift Buffer(4) , Right , 2 : Yaw0 = Buffer(4)
  Shift Buffer(5) , Right , 2 : Roll0 = Buffer(5)          '  High Bytes
  Shift Buffer(6) , Right , 2 : Pitch0 = Buffer(6)
End Sub

Sub Set_offset()
  _yawoffset = 0
  _rolloffset = 0
  _pitchoffset = 0
  For I = 1 To 100
      Call Read_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 Filter_gyro_data()
 Call Read_data()

 _yawnow = Yaw - _yawoffset_int
 _rollnow = Roll - _rolloffset_int
 _pitchnow = Pitch - _pitchoffset_int

 '_yawnow = _yawnow / 2
 '_rollnow = _rollnow / 2
 '_pitchnow = _pitchnow / 2

End Sub



Sub Pid_regulator()
 _yaw_err = _sbl(4) / 1
 _yaw_err = _yaw_err - _yawnow

 _roll_err = _sbl(3) / 5
 _roll_err = _roll_err - _rollnow

 _pitch_err = _sbl(2) / 5
 _pitch_err = _pitch_err - _pitchnow


 _yaw_kp_err = _yaw_kp_err * _yaw_kp
 _yaw_kp_err = _yaw_err / 10

 _roll_kp_err = _roll_kp_err * _roll_kp
 _roll_kp_err = _roll_err / 10

 _pitch_kp_err = _pitch_kp_err * _pitch_kp
 _pitch_kp_err = _pitch_err / 10


 _yaw_ki_err = _yaw_ki_err * _yaw_ki
 _yaw_ki_err = _yaw_err / 50
 _yaw_ki_sum = _yaw_ki_sum + _yaw_ki_err

 _roll_ki_err = _roll_ki_err * _roll_ki
 _roll_ki_err = _roll_err / 50
 _roll_ki_sum = _roll_ki_sum + _roll_ki_err

 _pitch_ki_err = _pitch_ki_err * _pitch_ki
 _pitch_ki_err = _pitch_err / 50
 _pitch_ki_sum = _pitch_ki_sum + _pitch_ki_err


 _yaw_kd_err = _yaw_kd_err * _yaw_kd
 _yaw_kd_err = _yaw_err / 50
 _yaw_kd_err = _yaw_kd_old - _yaw_kd_err
 _yaw_kd_old = _yaw_kd_old

 _roll_kd_err = _roll_kd_err * _roll_kd
 _roll_kd_err = _roll_err / 50
 _roll_kd_err = _roll_kd_old - _roll_kd_err
 _roll_kd_old = _roll_kd_old

 _pitch_kd_err = _pitch_kd_err * _pitch_kd
 _pitch_kd_err = _pitch_err / 50
 _pitch_kd_err = _pitch_kd_old - _pitch_kd_err
 _pitch_kd_old = _pitch_kd_old


 _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


 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 Filter_rx_data()

 For I = 1 To 4
  _sbl(i) = Kanal(i) - _empfmiddle(i)
  _sbl(i) = _sbl(i) / _empfdiv(i)
 Next I

 '_sbl(4) = _sbl(4) * -1

 _sbl(2) = _sbl(2) / 2
 _sbl(3) = _sbl(3) / 2
 _sbl(4) = _sbl(4) / 2

 'IIR-Filter
 'filter_wert = filter_wert * s + wert * (1.0-s)

 For I = 1 To 4
  _sbl_filter(i) = _sbl_filter(i) * 6
  _sbl_filter(i) = _sbl_filter(i) / 10
  _sbl_filter_2(i) = _sbl(i) * 4
  _sbl_filter_2(i) = _sbl_filter_2(i) / 10
  _sbl_filter(i) = _sbl_filter(i) + _sbl_filter_2(i)
  _sbl(i) = _sbl_filter(i)
 Next I

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

 _bl(1) = _bl(1) + _pitch_pid
 _bl(2) = _bl(2) + _roll_pid
 _bl(3) = _bl(3) - _roll_pid
 _bl(4) = _bl(4) - _yaw_pid
End Sub


Sub Send_data()
  _crc = Crc16(_bl(1) , 4)
  Printbin Start_byte ; _bl(1) ; _bl(2) ; _bl(3) ; _bl(4) ; _crc
End Sub


End

---

Ich kann mir das einfach nicht erklären, aber evtl. hat jemand von euch eine Erklärung dafür oder hat soetwas bei sich auch schon mal entdeckt?!
Gruß
Chris

EDIT:
Hier noch die Sprint-Layout Datei!

EDIT2:
Hat sich erledigt! Das WMP braucht anscheinend nach dem initialisieren eine gewisse Zeit, um brauchbare Werte zu liefern! Hab jetzt einfach

Code:

---

Call Wmp_init()
Call Filter_gyro_data()
Call Set_offset()

---

geschrieben. Dadurch werden die ersten Werte die das Teil liefert verworfen und dann funktionierts tadellos :)