' Compass_wpd_enabled.bs2
'
' Displays x (N/S) and y (W/E) axis measurements along with the direction the
' Compass Module is pointing, measured in degrees clockwise from north.
'
' THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF
' ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO
' THE IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A
' PARTICULAR PURPOSE.
'
' Copyright (c) Microsoft Corporation. All rights reserved
'
' {$STAMP BS2}
' {$PBASIC 2.5}
' ============================================================================

' -----[ Pins/Constants/Variables ]-------------------------------------------
DinDout        PIN      6                    ' P6 transceives to/from Din/Dout
Clk            PIN      5                    ' P5 sends pulses to HM55B's Clk
En             PIN      4                    ' P4 controls HM55B's /EN(ABLE)

Reset          CON      %0000                ' Reset command for HM55B
Measure        CON      %1000                ' Start measurement command
Report         CON      %1100                ' Get status/axis values command
Ready          CON      %1100                ' 11 -> Done, 00 -> no errors
NegMask        CON      %1111100000000000    ' For 11-bit negative to 16-bits

x              VAR      Word                 ' x-axis data
y              VAR      Word                 ' y-axis data
status         VAR      Nib                  ' Status flags
angle          VAR      Word                 ' Store angle measurement

SensorID  VAR   Byte  'Sensor identifier = 5 for PIR
ElementSize VAR Byte  'Size (in bytes) of each element
ElementCount  VAR   Byte  'Count of elements in packet
Padding VAR Byte      'Padding for the 8-byte element

SensorID = 1
ElementSize = 1
ElementCount = 3      '3-bytes for compass data;

NewInterval VAR  Word  'New interval requested by user
Interval  VAR   Word  'Interval value utlized by firmware

LFD       CON $10     'Linefeed character

Interval = 200         '.20 of a second interval
NewInterval = 200

' -----[ Main Routine ]-------------------------------------------------------

Main:
  GOSUB PollSensor                  'Was motion detected?
  GOSUB RetrieveInterval            'Retrieve units data

' -----[ Subroutines ]--------------------------------------------------------

Timeout:
  SEROUT 16, 16468, [DEC1 SensorID, DEC1 ElementSize, DEC1 ElementCount, DEC3 angle, DEC5 Interval,LFD]
 GOTO Main

PollSensor:                            ' Compass module subroutine

  HIGH En: LOW En                            ' Send reset command to HM55B
  SHIFTOUT DinDout,clk,MSBFIRST,[Reset\4]

  HIGH En: LOW En                            ' HM55B start measurement command
  SHIFTOUT DinDout,clk,MSBFIRST,[Measure\4]
  status = 0                                 ' Clear previous status flags

  DO                                         ' Status flag checking loop
    HIGH En: LOW En                          ' Measurement status command
    SHIFTOUT DinDout,clk,MSBFIRST,[Report\4]
    SHIFTIN  DinDout,clk,MSBPOST,[Status\4]  ' Get Status
  LOOP UNTIL status = Ready                  ' Exit loop when status is ready

  SHIFTIN  DinDout,clk,MSBPOST,[x\11,y\11]   ' Get x & y axis values
  HIGH En                                    ' Disable module

  IF (y.BIT10 = 1) THEN y = y | NegMask      ' Store 11-bits as signed word
  IF (x.BIT10 = 1) THEN x = x | NegMask      ' Repeat for other axis

  angle = x ATN -y                           ' Convert x and y to brads
  angle = angle */ 360                       ' Convert brads to degrees

  RETURN


RetrieveInterval:
    SERIN 16, 16468, Interval, Timeout, [DEC NewInterval]   'Retrieve interval
    IF NewInterval >= 10 AND NewInterval <= 60000 THEN
      Interval = NewInterval
    ENDIF
  RETURN