$regfile = "M8def.dat"
$crystal = 8000000
$baud = 19200
$hwstack = 200
$swstack = 200
$framesize = 400

Config Scl = Portb.7
Config Sda = Portb.5


Declare Sub Read_mag


Dim Tmp_magx(2) As Byte
Dim Tmp_magy(2) As Byte
Dim Tmp_magz(2) As Byte
Dim Temp As Byte

Dim Magx As Integer
Dim Magy As Integer
Dim Magz As Integer

Dim Magxx As Single
Dim Magyy As Single
Dim Magzz As Single


Wait 4


I2cinit


' -------------- TO DO ---------------------------------------------------------
'1. Reset MPU9150: write 0x80 to 0x6B (D107) (PWR_MGMT_1), address 0x68
'2. Disable sleep: write 0x01 to 0x6B (PWR_MGMT_1), address 0x68
'3. Setup I2C Bypass: write 0x02 to 0x37(D55) (INT_PIN_CFG), address 0x68
'4. Set mag to single measurement: write 0x01 to 0x0A (CNTL), address 0x0C
'5. read mag: Read 6 bytes from 0x03 (HXL), address 0x0C


   '1. --- (107) Power Management 1 RESET ---
   I2cstart
   I2cwbyte &HD0                                            'HEX Schreibadresse MPU-9150
   I2cwbyte 107                                             'DEZ Register 107 Power Management 1
   I2cwbyte &B10000000                                      'BIN Reset / No Sleep / No Cycle / Temp_Sens: Dis / Clock Source: Z-Gyro
   I2cstop


   '2. --- (107) Power Management 1 DISABLE SLEEP (wake up) ---
   I2cstart
   I2cwbyte &HD0
   I2cwbyte 107
   I2cwbyte &B00000001
   I2cstop


   '3. --- (55) INT_PIN_CFG BYPASS ---
   I2cstart
   I2cwbyte &HD0
   I2cwbyte 55                                              'DEZ Register 55 INT_PIN_CFG
   I2cwbyte &B00000010                                      'BIN Bypass enabled
   I2cstop



'4. Set mag to single measurement: write 0x01 to 0x0A (CNTL), address 0x0C
'5. read mag: Read 6 bytes from 0x03 (HXL), address 0x0C



End



Sub Read_mag()

   I2cstart

   I2cwbyte &HD0
   I2cwbyte 3                                               'HEX Start XL,XH,YL,YH,ZL,ZH
   I2crepstart
   I2cwbyte &HD1

   I2crbyte Tmp_magx(1) , Ack
   I2crbyte Tmp_magx(2) , Ack

   I2crbyte Tmp_magy(1) , Ack
   I2crbyte Tmp_magy(2) , Ack

   I2crbyte Tmp_magz(1) , Ack
   I2crbyte Tmp_magz(2) , Nack

   I2cstop

End Sub