;------------------------------------------------------------------------------
; Source code for a quick digital compass checker.  This routine simply
;  gets a compass reading, checks the value in c_speed, and stores the
;  correct heading code into c_heading, depending on the speed.
;
; Rev 1.0, 11-29-00
; Kevin Nichols, CSE466
; 
; Port Usage:
; Port 2 used for the four compass wires: (P2.7 = N, P2.6 = E, P2.5 = S, P2.4 = W)
;
;
;------------------------------------------------------------------------------
$NOMOD51		; disable predefined 8051 registers
$INCLUDE (At89c55.INC)	; include CPU definition file for Atmel 89C55


;------------------------------------------------------------------------------
; Module name
;------------------------------------------------------------------------------
NAME		Compass_Tester


;------------------------------------------------------------------------------
; IDATA SEGMENT--Reserves space in INDIRECT DATA RAM (upper 128 bytes)--
;------------------------------------------------------------------------------
VARS2		SEGMENT	IDATA		; segment for DATA RAM.
		RSEG	VARS2		; switch to this data segment

c_heading:	DS	1		; (servo) Heading converted into a 0-180 value.
c_speed:	DS	1		; (servo) Speed (in tenths of knots) converted into 1 byte.


;------------------------------------------------------------------------------
; CONSTANTS (typeless)
;------------------------------------------------------------------------------
compass_port	EQU	P2		; Port used for digital compass (P2.7 = N, P2.6 = E, P2.5 = S, P2.4 = W)



;------------------------------------------------------------------------------
; Put the STACK segment in the main module.
;------------------------------------------------------------------------------
STACK1		SEGMENT	IDATA		; STACK1 goes into IDATA RAM.
		RSEG	STACK1		; switch to STACK1 segment.
		DS	24		; reserve 24 bytes stack space

;------------------------------------------------------------------------------
; LJMP to "start"
;------------------------------------------------------------------------------
		CSEG	AT	0	; absolute Segment at Address 0
		LJMP	start		; reset location (jump to start)


;------------------------------------------------------------------------------
; CODE SEGMENT--Reserves space in CODE ROM for assembler instructions.
;------------------------------------------------------------------------------
code_seg_name	SEGMENT	CODE
		RSEG	code_seg_name	; switch to this code segment
		USING	0		; Use register bank 0  


start:		MOV	SP,#STACK1-1	; assign stack at beginning
		
		
		CALL	decide_compass	; Run the compass test routine
		
loop:		SJMP	loop		; Busywait




;------------------------------------------------------------------------------
; SUBROUTINES
;------------------------------------------------------------------------------
sub_segment	SEGMENT	CODE		; segment for interrupt function
		RSEG	sub_segment	; switch to this code segment
		USING	0		; register bank for subroutines;

;--------------------------------------------------------------------------------------------------------
; Subroutine to check current speed, and decide whether to use compass (if speed < 1.6 knots) or use GPS
; Also converts compass input pins to heading values.
		
decide_compass:	MOV	R0,#c_speed			;Point to the speed
		MOV	A,@R0				;Grab the speed
		ANL	A,#0xF0				;Drop the low nibble
		CJNE	A,#0x00,return			;If speed was >= 1.6 knots (= 0x0F), then leave GPS value in c_heading
							;otherwise, replace the c_heading value with the compass value...

		MOV	R0,#c_heading			;Point at the heading var
		MOV	A,compass_port			;Grab the compass port
		ANL	A,#0xF0				;Mask off the lower nibble (used by the IPC)

		CJNE	A,#0x70,case_E
;case N				
		MOV	@R0,0x00			;Data for North (0)
		JMP	compass_1

case_E:		CJNE	A,#0xB0,case_S
		MOV	@R0,#0x2D			;Data for East (45)
		JMP	compass_1

case_S:		CJNE	A,#0xD0,case_W
		MOV	@R0,#0x5A			;Data for South (90)
		SJMP	compass_1


case_W:		CJNE	A,#0xE0,case_NE
		MOV	@R0,#0x87			;Data for W (135)
		SJMP	compass_1

case_NE:	CJNE	A,#0x30,case_SE
		MOV	@R0,#0x16			;Data for NE (22)
		SJMP	compass_1

case_SE:	CJNE	A,#0x90,case_SW
		MOV	@R0,#0x4D			;Data for SE (77)
		SJMP	compass_1

case_SW:	CJNE	A,#0xC0,case_NW
		MOV	@R0,#0x70			;Data for SW (112)
		SJMP	compass_1

case_NW:	CJNE	A,#0x60,compass_1		;If compass is bad, then just return (& use GPS)
		MOV	@R0,#0x9D			;Data for NW (157)
		SJMP	compass_1



compass_1:	; Can put stuff here to do averaging of compass reading (in the future)

		RET