MPU6000 übernimmt Register nicht

Hi,

ich versuche, einen MPU6000 (3x Gyro & 3x ACC) zu konfigurieren. Dazu möchte ich den Wert &H00 in das Register 28 schreiben. Leider wird aber der Wert nicht übernommen und das Register enthält immer den Wert 98. Der Code:

Code:

---

$regfile = "xm32a4def.dat"
$crystal = 32000000
$framesize = 150                                            '100
$swstack = 150                                              '100
$hwstack = 150                                              '100


$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 Init_mpu()
Declare Sub Read_gyro()
Declare Sub Read_acc()

Declare Sub Get_gyro_offset()

'--UART Settings--
Config Com2 = 115200 , Mode = Asynchroneous , Parity = None , Stopbits = 1 , Databits = 8
'Config Serialin1 = Buffered , Size = 254
'Config Serialout1 = Buffered , Size = 250
Open "COM2:" For Binary As #1

'Clear Serialin1
'Clear Serialout1


'--TWI Settings--
Dim Twi_start As Byte
Open "twie" For Binary As #2
Config Twie = 400000
I2cinit #2


'--MPU 60X0--
Const Mpuaddw = &B11010000
Const Mpuaddr = &B11010001

Dim Gyrox As Long
Dim Gyroy As Long
Dim Gyroz As Long

Dim Accx As Long
Dim Accy As Long
Dim Accz As Long

Dim Gx As Integer
Dim Gy As Integer
Dim Gz As Integer
Dim Tmp_gyrox(2) As Byte At Gx Overlay
Dim Tmp_gyroy(2) As Byte At Gy Overlay
Dim Tmp_gyroz(2) As Byte At Gz Overlay
Dim Ax As Integer
Dim Ay As Integer
Dim Az As Integer
Dim Tmp_accx(2) As Byte At Ax Overlay
Dim Tmp_accy(2) As Byte At Ay Overlay
Dim Tmp_accz(2) As Byte At Az 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 Accz_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


Dim I As Byte

Dim Test As Byte


Dim Acc_conf As Byte
Acc_conf = 0


Const Offset_schwelle = 3


'--Init Gyro & ACC--
Waitms 500
Call Init_mpu()
'Waitms 100
'Call Init_mpu()
'Waitms 100
'Call Init_mpu()
Waitms 250


Wait 4


I2cstart #2
I2cwbyte Mpuaddw , #2
I2cwbyte 28 , #2
I2crepstart #2
I2crbyte Test , Nack , #2
I2cstop #2

While Test <> Acc_conf

  I2cstart #2
  I2cwbyte Mpuaddw , #2
  I2cwbyte 28 , #2
  I2cwbyte Acc_conf , #2
  I2cstop #2

  I2cstart #2
  I2cwbyte Mpuaddw , #2
  I2cwbyte 28 , #2
  I2crepstart #2
  I2crbyte Test , Nack , #2
  I2cstop #2

  Print #1 , Test

Wend

Print #1 , "ACC-Config = " ; Bin(test)



Call Get_gyro_offset()




Do


  Call Read_acc()
  Print #1 , Accx ; " : " ; Accy ; " : " ; Accz
  Waitms 100


Loop

End


Sub Get_gyro_offset()


  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 >= Offset_schwelle 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 >= Offset_schwelle 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 >= Offset_schwelle Then Movement = 1
      Next I

      Compare_tmp = Roll_check(1) - Roll_check(50)
      Compare_tmp = Abs(compare_tmp)
      If Compare_tmp >= Offset_schwelle Then Movement = 1
      Compare_tmp = Roll_check(1) - Roll_check(25)
      Compare_tmp = Abs(compare_tmp)
      If Compare_tmp >= Offset_schwelle Then Movement = 1

      Compare_tmp = Nick_check(1) - Nick_check(50)
      Compare_tmp = Abs(compare_tmp)
      If Compare_tmp >= Offset_schwelle Then Movement = 1
      Compare_tmp = Nick_check(1) - Nick_check(25)
      Compare_tmp = Abs(compare_tmp)
      If Compare_tmp >= Offset_schwelle Then Movement = 1

      Compare_tmp = Yaw_check(1) - Yaw_check(50)
      Compare_tmp = Abs(compare_tmp)
      If Compare_tmp >= Offset_schwelle Then Movement = 1
      Compare_tmp = Yaw_check(1) - Yaw_check(25)
      Compare_tmp = Abs(compare_tmp)
      If Compare_tmp >= Offset_schwelle Then Movement = 1

  Wend


  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



  'Accx_offset = 110
  'Accy_offset = 55
  'Accz_offset = 8192
  Accx_offset = 0
  Accy_offset = 0
  Accz_offset = 0


End Sub


Sub Init_mpu()


  I2cstart #2                                              'Reset MPU60X0
  I2cwbyte Mpuaddw , #2
  I2cwbyte 107 , #2
  I2cwbyte &B10000000 , #2
  I2cstop #2

  Waitms 10

  I2cstart #2                                              'Set Samplerate Register
  I2cwbyte Mpuaddw , #2
  I2cwbyte 25 , #2
  I2cwbyte &B00000000 , #2
  I2cstop #2

  I2cstart #2                                              'Set Powermanagement Register
  I2cwbyte Mpuaddw , #2
  I2cwbyte 107 , #2
  I2cwbyte &B00001011 , #2
  I2cstop #2

  I2cstart #2                                              'Set Gyro-Config to 2000°/s
  I2cwbyte Mpuaddw , #2
  I2cwbyte 27 , #2
  I2cwbyte &B00011000 , #2
  I2cstop #2

  I2cstart #2                                              'Set ACC-Config to +/-2g
  I2cwbyte Mpuaddw , #2
  I2cwbyte 28 , #2
  I2cwbyte Acc_conf , #2                                  '0
  I2cstop #2

  I2cstart #2                                              'Set DLPF-Config to 42/44 Hz
  I2cwbyte Mpuaddw , #2
  I2cwbyte 26 , #2
  I2cwbyte &B00000011 , #2
  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 = Gx
  Gyroy = 0 - Gy
  Gyroz = 0 - Gz

  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) , Ack , #2

  I2crbyte Tmp_accz(2) , Ack , #2
  I2crbyte Tmp_accz(1) , Nack , #2

  I2cstop #2

  Accx_offset = 0
  Accy_offset = 0
  Accz_offset = 0

  Accx = Ax
  Accy = Ay
  Accz = Az

  Accx = Accx - Accx_offset
  Accy = Accy - Accy_offset
  Accz = Accz - Accz_offset


End Sub

---

Hat jemand vielleicht sowas ähnliches schonmal beobachtet und hat einen Tipp für mich?

Vielen Dank & Gruß
Chris