Option base 0
Option default none
Option explicit on

Const qmc5883l.I2C_ADDR% = &h0D

SetPin GP4, GP5, I2C
I2C OPEN 100, 1000

Dim mag_x%, mag_y%, mag_z%, temp%

Print "Initialising..."
qmc5883l.init()

Print "Running..."
Do
'  qmc5883l.read_all()
qmc5883l.read(mag_x%, mag_y%, mag_z%, temp%)
Print mag_x%, mag_y%, mag_z%, temp%
Pause 1000
Loop

' Initialises the QMC5883L.
Sub qmc5883l.init()

' Reset the chip.
qmc5883l.write_register(&h0A, &h80)
qmc5883l.read_all()

qmc5883l.write_register(&h0B, &h01)
qmc5883l.read_all()

' Continuous (0x01), 200Hz (0x0C), 8G (0x10), OSR=512 (0x00)
' qmc5883l.write_register(&h09, &h01 Or &h0C Or &h10 Or &h00)
qmc5883l.write_register(&h09, &h1D)
qmc5883l.read_all()
End Sub

' Writes value to a QMC5883L register.
Sub qmc5883l.write_register(register%, value%)
' Address the slave and write two bytes, the register and the value.
I2C Write qmc5883l.I2C_ADDR%, 0, 2, register%, value%
If MM.I2C Then Error qmc5883l.get_i2c_error$()
End Sub

' Gets string corresponding to MM.I2C error code.
Function qmc5883l.get_i2c_error$()
Select Case MM.I2C
  Case 0    : qmc5883l.get_i2c_error$ = "None"
  Case 1    : qmc5883l.get_i2c_error$ = "NACK"
  Case 2    : qmc5883l.get_i2c_error$ = "Timeout"
  Case Else : qmc5883l.get_i2c_error$ = "Unexpected error: " + Str$(MM.I2C)
End Select
End Function

' Reads X, Y, Z from the magnetic sensor and also the temperature.
Sub qmc5883l.read(mag_x%, mag_y%, mag_z%, temp%)
' TODO: should probably check if data ready bit/register is set.

' Address the slave and send the first register address to read (0x00).
I2C Write qmc5883l.I2C_ADDR%, 1, 1, &h00
If MM.I2C Then Error qmc5883l.get_i2c_error$()

' Read 8 registers/bytes (0x00 .. 0x07).
Local buf$
I2C Read qmc5883l.I2C_ADDR%, 0, 8, buf$
If MM.I2C Then Error qmc5883l.get_i2c_error$()

mag_x% = Peek(Var buf$, 1) Or (Peek(Var buf$, 2) << 8)
mag_y% = Peek(Var buf$, 3) Or (Peek(Var buf$, 4) << 8)
mag_z% = Peek(Var buf$, 5) Or (Peek(Var buf$, 6) << 8)
temp%  = Peek(Var buf$, 8) Or (Peek(Var buf$, 9) << 8)
End Sub

' Reads and dumps the contents of all 14 QMC5883L registers.
Sub qmc5883l.read_all()
' Address the slave and send the first register address to read (0x00).
I2C Write qmc5883l.I2C_ADDR%, 1, 1, &h00
If MM.I2C Then Error qmc5883l.get_i2c_error$()

' Read 14 registers/bytes (0x00 .. 0x0D).
Local buf$
I2C Read qmc5883l.I2C_ADDR%, 0, 14, buf$
If MM.I2C Then Error qmc5883l.get_i2c_error$()

Local i%
For i% = 1 To 14
'    Print Hex$(Asc(Mid$(buf$, i%, 1)), 2) " ";
  Print Asc(Mid$(buf$, i%, 1));
Next
Print
End Sub

' i2cdetect like program

' Set Pins!
SetPin gp4, gp5, I2C

' Open first I2C
I2C open 100, 400


' Prep table
Print "     0  1  2  3  4  5  6  7  8  9  A  B  C  D  E  F"

' loop col/row
For y=0 To 7
Print Hex$(y,2);": ";
For x=0 To 15
 addr = y*16+x               ' calc address
 I2C write addr, 0, 1, &H00  ' write zerp to that adress
 If MM.I2C=0 Then            ' check for errors
   Print Hex$(addr,2);" ";   ' found one!
Else
  Print "-- ";               ' nothing here..
EndIf
Next x
Print
Next y

I2C close  ' clean up