I2C Rev0.01

OBJ

  pst          : "FullDuplexSerial"
  alt          : "altimeter_29124"

CON

  _clkmode      = xtal1 + pll16x
  _clkfreq      = 80_000_000

  SDApin         = 0            ' SDA of gyro connected to P0
  SCLpin         = 1            ' SCL of gyro connected to P1
  WRITE         = $D2           ' Request Write operation
  READ          = $D3           ' Request Read operation

  ' Control registers
  CTRL_REG1     = $20
  CTRL_REG2     = $21
  CTRL_REG3     = $22
  CTRL_REG4     = $23
  STATUS_REG    = $27
  OUT_X_INC     = $A8

  x_idx = 0
  y_idx = 1
  z_idx = 2

  START_ALT     = 200             ' Your starting altitude in feet

  WRITE_DATA     = $3C ' Requests Write operation
  READ_DATA      = $3D ' Requests Read operation
  MODE           = $02 ' Mode setting register
  OUTPUT_X_MSB   = $03 ' X MSB data output register

VAR

  long x
  long y
  long z
  long a
  long b
  long c

  long cx
  long cy
  long cz

  long ff_x
  long ff_y
  long ff_z

  long multiBYTE[3]

PUB Main | last_ticks, d

  pst.start(31, 30, 0, 115200)

  ' Set modes
  Wrt_1B(CTRL_REG3, $08)                    ' Data ready signal
  Wrt_1B(CTRL_REG4, $80)                    ' Block data update
  Wrt_1B(CTRL_REG1, $1F)                    ' Enable all axes

  last_ticks := cnt
  SetCont
  alt.start(alt#QUICKSTART, alt#BACKGROUND)
  alt.set_resolution(alt#HIGHEST)
  alt.set_altitude(alt.m_from_ft(START_ALT * 100))

  repeat
    pst.tx(1)                               ' Set Terminal data
    WaitForDataReady                            '  at top of screen
    Read_MultiB(OUT_X_INC)                  ' Read XYZ bytes
    SetPointer(OUTPUT_X_MSB)
    getRaw                           ' Gather raw data from compass
    pst.tx(13)
    pst.tx(13)
    d := alt.altitude(alt.average_press)
    pst.str(string("Altitude:"))
    pst.str(alt.formatn(d, alt#METERS | alt#CECR, 8))
    pst.str(alt.formatn(d, alt#TO_FEET | alt#CECR, 17))
    ' Divide by 114 to reduce noise
    x := (x - cx) / 114
    y := (y - cy) / 114
    z := (z - cz) / 114
    pst.tx(13)
    RawXYZ
    WaitCnt(ClkFreq / 4  + Cnt)             ' Delay before next loop

PUB RawXYZ                     'gyroscope

  pst.str(string("RAW X ",11))
  pst.dec(x)
  pst.str(string(13, "RAW Y ",11))
  pst.dec(y)
  pst.str(string(13, "RAW Z ",11))
  pst.dec(z)

PUB GetRaw            'compass

  start
  send(READ_DATA)
  a := ((receive(true) << 8) | receive(true))
  b := ((receive(true) << 8) | receive(true))
  c := ((receive(true) << 8) | receive(false))
  stop
  ~~a
  ~~c
  ~~b
  a := a
  c := c
  b := b
  pst.str(string("X="))
  pst.dec(a)
  pst.str(string(", Y="))
  pst.dec(b)
  pst.str(string(", Z="))
  pst.dec(c)
  pst.str(string("    "))

PUB SetCont
  ' Sets compass to continuous output mode

  start
  send(WRITE_DATA)
  send(MODE)
  send($00)
  stop

PUB SetPointer(Register)
  ' Start pointer at user specified register (OUT_X_MSB)

  start
  send(WRITE_DATA)
  send(Register)
  stop
PUB WaitForDataReady | status
    repeat
      status := Read_1B(STATUS_REG)
      if (status & $08) == $08
        quit

PUB Wrt_1B(SUB1, data)
  ''Write single byte to Gyroscope.

      start
      send(WRITE)
      send(SUB1)
      send(data)
      stop

PUB Read_1B(SUB3) | rxd
 ''Read single byte from Gyroscope

      start
      send(WRITE)
      send(SUB3)
      stop

      start
      send(READ)
      rxd := receive(false)
      stop

     result := rxd

PUB Read_MultiB(SUB3)
 ''Read multiple bytes from Gyroscope

      start
      send(WRITE)
      send(SUB3)
      stop

      start
      send(READ)
      multiBYTE[x_idx] := (receive(true)) |  (receive(true)) << 8
      multiBYTE[y_idx] := (receive(true)) |  (receive(true)) << 8
      multiBYTE[z_idx] := (receive(true)) |  (receive(false)) << 8
      stop

      x := ~~multiBYTE[x_idx]
      y := ~~multiBYTE[y_idx]
      z := ~~multiBYTE[z_idx]

PRI send(value)

  value := ((!value) >< 8)

  repeat 8
    dira[SDApin]       := value
    dira[SCLpin]       := false
    dira[SCLpin]       := true
    value >>= 1

  dira[SDApin]         := false
  dira[SCLpin]         := false
  result               := not(ina[SDApin])
  dira[SCLpin]         := true
  dira[SDApin]         := true

PRI receive(acknowledge)

  dira[SDApin]         := false

  repeat 8
    result <<= 1
    dira[SCLpin]       := false
    result             |= ina[SDApin]
    dira[SCLpin]       := true

  dira[SDApin]         := (acknowledge)
  dira[SCLpin]         := false
  dira[SCLpin]         := true
  dira[SDApin]         := true

PRI start

  outa[SDApin]         := false
  outa[SCLpin]         := false
  dira[SDApin]         := true
  dira[SCLpin]         := true

PRI stop

  dira[SCLpin]         := false
  dira[SDApin]         := false