* ******************************************************************
* By Andrew Chanler - FALL 2003
* Handyboard driver for using Devantech Magnetic Compass - CMPS03
* refer to http://www.cs.uml.edu/~achanler/robotics/
* ******************************************************************


* 6811 registers

ADCTL		equ	$1030	; A/D Control/status Register
ADR1		equ	$1031	; A/D Result Register 1

TCTL2 		equ	$1021
TFLG1 		equ 	$1023
TMSK1 		equ 	$1022 
TCNT 		equ 	$100e

	org	MAIN_START

variable_start_time		FDB 00
variable_compass_wrap	FDB 00

* take analog reading with interrupts disabled	
subroutine_compass_driver:

* turn off interrupts
*	sei
	
	clra
	clrb
	std variable_compass_wrap
	

* Disable input capture interrupt on TIC3
* bit_clear(TMSK1, 0b00000001);
	ldaa	TMSK1
	anda	#%11111110
	staa	TMSK1

* Disable input capture recording on TIC3
* bit_clear(TCTL2, 0b00000011);
	ldaa	TCTL2
	anda	#%11111100
	staa	TCTL2

* Clear past input capture on TIC3
* poke(TFLG1, 0b00000001);
	ldaa	#%00000001
	staa	TFLG1

* Enable rising edge input capture recording on TIC3
* bit_set(TCTL2, 0b00000010);
	ldaa	TCTL2
	ora	#%00000001
	staa	TCTL2

cloop:
	ldaa TFLG1
	anda #%00000001
	beq cloop
	
	ldd 0x1014;TIC3
	std variable_start_time

	sei
* Disable input capture recording on TIC3
* bit_clear(TCTL2, 0b00000011);
	ldaa	TCTL2
	anda	#%11111100
	staa	TCTL2

* Clear past input capture on TIC3
* poke(TFLG1, 0b00000001);
	ldaa	#%00000001
	staa	TFLG1

* Enable falling edge input capture recording on TIC3
* bit_set(TCTL2, 0b00000010);
	ldaa	TCTL2
	ora	#%00000010
	staa	TCTL2	

wait:
	ldd TCNT
	ldab variable_start_time
	cba
	beq wait


cloop2:
	ldd TCNT
	ldab variable_start_time

	cba
	bne no_wrap
	
	ldaa #1
	staa variable_compass_wrap+1

no_wrap:
	ldaa TFLG1
	anda #%00000001
	beq cloop2
	
* Disable input capture recording on TIC3
* bit_clear(TCTL2, 0b00000011);
	ldaa	TCTL2
	anda	#%11111100
	staa	TCTL2

	cli
	
	ldd 0x1014;TIC3
	subd variable_start_time
	rts			; d contains timer start value