; .\common.SRC generated from: .\common.c


;%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
;  The following lines are included to check the compilability of this file %
$NOMOD51			; disable predefined 8051 registers
$INCLUDE (At89c55.INC)		; include CPU definition file for Atmel 89C55
;%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
;
;!!!!%%% Put the wait for good sentence back in the serial port when test is done!


NAME	COMMON

?PR?_asm_set_servo?COMMON               SEGMENT CODE
?DT?_asm_set_servo?COMMON               SEGMENT DATA OVERLAYABLE
?PR?asm_get_waypoint?COMMON             SEGMENT CODE
?PR?asm_send_waypoint?COMMON            SEGMENT CODE
;?DT?asm_send_waypoint?COMMON           SEGMENT DATA OVERLAYABLE
?PR?asm_init?COMMON  			SEGMENT CODE
?PR?asm_do_isr?COMMON  			SEGMENT CODE
?PR?asm_lcd_print?COMMON               SEGMENT CODE
?DT?asm_lcd_print?COMMON               SEGMENT DATA OVERLAYABLE
	PUBLIC	asm_lcd_print

	PUBLIC	asm_init
	PUBLIC	asm_send_waypoint
	PUBLIC	asm_get_waypoint

	PUBLIC	_asm_set_servo

	PUBLIC	c_speed
	PUBLIC	c_heading
	PUBLIC	d_lon
	PUBLIC	c_lon
	PUBLIC	d_lat
	PUBLIC	c_lat	
	PUBLIC	message


	PUBLIC	asm_do_isr

	RSEG  ?DT?_asm_set_servo?COMMON
?_asm_set_servo?BYTE:
command_byte:	DS	1		; (Servo) Holds the command byte [CCCCCAAA]
servo_data:	DS	1		; (Servo) Holds data for this command
bit_count:	DS	1		; (Servo) Counter for number of bits being sent
buffer_h:	DS	1		; (Servo) Buffer high byte (initially contains servo data)
buffer_l:	DS	1		; (Servo) Buffer low byte (initially contains command & address)

;	RSEG  ?DT?asm_send_waypoint?COMMON
;?asm_send_waypoint?BYTE:
;   the_data?342:   DS   1

	RSEG  ?DT?asm_lcd_print?COMMON
?asm_lcd_print?BYTE:
   data_ptr?643:   DS   1


;------------------------------------------------------------------------------
; DATA SEGMENT--Reserves space in DATA RAM (lower 128 bytes)--
;------------------------------------------------------------------------------
VARS		SEGMENT	DATA		; segment for DATA RAM.
		RSEG	VARS		; switch to this data segment

code_buf_ptr:	DS	1		; (serial) Stores the address of the next free code buffer location.
char_buf_ptr:	DS	1		; (serial) Stores the address of the next free character buffer location.
code_buf_cntr:	DS	1		; (serial) Stores the maximum number of bytes that can be put in the code_buffer
char_buf_cntr:	DS	1		; (serial) Stores the maximum number of bytes that can be put in the char_buffer
parse_key_cntr:	DS	1		; (Serial) Stores how far we are into the parse_key table (0 to n)
temp_sum_h:	DS	1		; (several) Stores the temporary sums for calculations in various routines
temp_sum_l:	DS	1
test_only_disp:	DS	5		; TEST ONLY!!! hold ASCII data to display

M		SEGMENT	DATA
		RSEG	M
message:	DS	25		; TEST ONLY!!!!


;------------------------------------------------------------------------------
; 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

code_buffer:	DS	6		; (Servo) Contains the first 6 chars from each sentence received,
					; (Servo) So that we can save on RAM by being pickey about which sentences we store.		
char_buffer:	DS	80		; (Servo) Our 80 character buffer for raw ASCII GPS data.  Remember to change init function if this size changes!
raw_latitude:	DS	5		; (Servo) North latitude.  Ex: 4751.698 (47 deg, 51.698 minutes) would be stored as 51698
raw_longitude:	DS	5		; (Servo) West longitude.  Ex: 12209.774 (122 deg, 09.774 minutes) would be stored as 09774
raw_speed:	DS	3		; (Servo) In tenths of knots.  Ex: 312.7 knots would be stored as 127
raw_heading:	DS	3		; (Servo) Heading w/r to True North. Ex: 237.8 degrees would be stored as 237

		ISEG	AT 0x80	; Non-relocatable segment for our struct.
c_lon:		DS	2		; (servo) Longitude converted into 16 bits.
		ISEG	AT 0x82
c_lat:		DS	2		; (servo) Latitude converted into 16 bits.
		ISEG	AT 0x84
c_heading:	DS	1		; (servo) Heading converted into a 0-180 value.
		ISEG	AT 0x85
c_speed:	DS	1		; (servo) Speed (in tenths of knots) converted into 1 byte.
		ISEG	AT 0x86
d_lon:		DS	2		; Desired longitude
		ISEG	AT 0x88
d_lat:		DS	2		; Desired latitude



;------------------------------------------------------------------------------
; BIT SEGMENT--Reserves space in BIT RAM
;------------------------------------------------------------------------------
bit_flags	SEGMENT	BIT		; segment for BIT RAM.
		RSEG	bit_flags	; switch to this bit segment

good_s:		DBIT	1		; (servo) Set means we're in the process of receiving a sentence that we want
s_avail:	DBIT	1		; (Servo) Set means a complete sentence is now available in the buffer
code_buff:	DBIT	1		; (Servo) Set means characters should be stored in the code buffer.
ignore:		DBIT	1		; (Servo) Set means we want to ignore characters (we still check for "$" though)
overflow:	DBIT	1		; (Servo) Set means that the char buffer has overflowed
ipc_data:	DBIT	1		; (ipc) This is the data bit we've just received or sent

;---------------------------------------------------------------------------
; ROM Lookup Table
;---------------------------------------------------------------------------
CONST		SEGMENT	CODE
		RSEG	CONST

sentence:	DB	'$GPRMC'	; Put the sentence here that we're looking for

; The parse key is used to parse the string into the characters you are interested in
; Values:  	A comma "," means to skip characters until you get to a comma, then advance to next char
;		An "S" means to skip that single character, & advance to next char
;		An "R" means to read that character into the ram location the current pointer is pointing at
;		An "E" means we're done parsing (return from subroutine.)
;		NOTE:  The RAM locations to be filled MUST BE CONTIGUOUS, and in the proper order (ie the first
;		raw_xxxx that goes with the first bunch of R's must be DS'd first, then the second, etc.) 
;		Obviously, the number of chars to be stored must match the amount of RAM DS'd for each variable exactly.
parse_key:	DB	',,,SSRRSRRR,,SSSRRSRRR,,SRRSR,RRRE'


;------------------------------------------------------------------------------
; CONSTANTS (typeless)
;------------------------------------------------------------------------------
YGM		EQU	P0_5		; (Servo) YGM (output) "You've Got Mail" line going to Motorola chip
DAT		EQU	P0_4		; (Servo) DAT (output) Data bit going to the Motorola chip
ACK		EQU	P0_3		; (Servo) ACK (input) from the Motorola chip
rs		EQU	P0_1		; (LCD) Port P0.1 RS line (1 = data, 0 = instructions)
e_line		EQU	P0_0		; (LCD) Port P0.0 Enable line (1 = enable)
lcd_data	EQU	P1		; (LCD) Port P1 (P1.4 - P1.7) are LCD data lines
IPC_DAT		EQU	P2_0		; Interprocessor comm line
IPC_ACK		EQU	P2_1		; Interprocessor comm line
IPC_CLK		EQU	P2_2		; Interprocessor comm line
compass_port	EQU	P2		; Port used for digital compass (P2.7 = N, P2.6 = E, P2.5 = S, P2.4 = W)





;======================================================================================================


; /* SET_SERVO:
;   cccccaaa:  Commands: 0=set position 
;   Data: 0 = Hard Left (CCW)  255 = Hard Right (CW)
;   Address: Address of servo for this call (0-7)*/
;
; void asm_set_servo(unsigned char c_a, unsigned char servo_data) {

		RSEG  ?PR?_asm_set_servo?COMMON
_asm_set_servo:
		USING	0

		MOV	command_byte,R7		; Move command/Adr byte into RAM
		MOV	servo_data,R5		; Move Data byte into RAM
;		CALL	send_package		; Send the package to the servo controller

?C0001:	
		RET

;(servo) send_package subroutine
;-------------------------------------------------
; Used to send a 16 bit package to the Motorola 68HC705 servo controller
; Assumes the data to be sent is already in command_byte and servo_data
;
; Sends 16 bits, high bit of buffer_h first (data) through low bit of buffer_l last (command / address).
;
send_package:	MOV	bit_count,#0x11		; Make sure bit counter starts with 17 (ie 16 bits to transfer.)
		MOV	buffer_h,servo_data	; Copy the stuff that's going out (so that the last servo data,
		MOV	buffer_l,command_byte	; address & command sent are always available if needed)
		
send_loop:	MOV	C,ACK		; Get the ACK bit
		JNC	ckYGM_1		; If ACK is clear, go check YGM (we'll either wait, or send a new bit)
					; To get here, ACK must be set.
					; If YGM is clear, we need to wait for servo controller chip to clear its ACK.
					; If YGM is set, then ball's in our court.  We need to clear YGM.
					; Either way, we can clear YGM and go back to top.
		CLR	YGM		; Clear YGM.
		SJMP	send_loop	; So, ACK set & YGM clear.  Now we're just waiting for the ACK line to go low.
	

ckYGM_1:	MOV	C,YGM		; So, ACK is clear.  Get YGM bit
		JNC	send_bit	; If YGM clear (& ACK is clear), then OK to send next bit.
					; ACK clear & YGM is set.  Means we're waiting for servo controller to ACK our data.
		SJMP	send_loop	; Don't need to do anything.  Just wait for servo controller to ACK our last bit.

		
send_bit:	DJNZ	bit_count,send_bit_1	; Check the number of bits still to send.  Is it zero?
		RET				; If so, we're done!!

send_bit_1:	MOV	A,buffer_l	; Still more bits to send.  ACK clear, YGM clear.  We're OK to send next bit.
		CLR	C		; Clear out the carry flag
		RLC	A		; Move 0 into lsb of buffer_l, and msb of buffer_l into carry
		MOV	buffer_l,A	; Save new buffer_l value
		MOV	A,buffer_h	; Get buffer_h
		RLC	A		; Move msb of buffer_l (was in carry) into lsb of buffer_h, and msb of buffer_h into carry
		MOV	DAT,C		; Put our bit on the DAT line.
		MOV	buffer_h,A	; Save modified copy of buffer_h
		SETB	YGM		; Set the "You've got mail" line.
		SJMP	send_loop


;(servo) long_delay routine.
;--------------------------
; To use:  Load variable R1 with some number (FF = approx 4 seconds)
; then call this subroutine.  Routine will return after specified amount of time.
; Number of cycles for each instruction shown in comments
		
long_delay:	MOV	R0,#0xFF	;1 = 375ns
outer_lp:	MOV	R2,#80		;1 = 375ns
inner_lp:	DJNZ	R2,inner_lp	;2 = 750ns (do this 80 times, total of 60us)
		DJNZ	R0,outer_lp	;2 = 750ns (do this 256 times, total of 15.648ms)
		DJNZ	R1,long_delay 	;2 = 750ns (do this R1 times)
		RET			;2 = 750ns
; END OF _asm_set_servo


;======================================================================================================


; Serial interrupt routine
	RSEG  ?PR?asm_do_isr?COMMON
asm_do_isr:
serial_isr:

		MOV	A,SBUF			; Get the character
		
		CJNE	A,#'$',no_start_chr	; Did we get a "$"?
						; Yes, we did get a "$"
		JNB	good_s,load_buffer	; Is the good_s bit set?
		CLR	good_s			; Yes, the good_s bit is set, so clear it
		SETB	s_avail			; and set the sentence available flag
		MOV	R0,char_buf_ptr		; Get current value of the char pointer
		MOV	@R0,#0x00		; Put in the end flag "0" for the LCD display

load_buffer:	MOV	R0,#code_buffer		; Get the address of code_buffer (to access indirect RAM)
		MOV	@R0,A			; Put the "$" into position 0 of the code buffer (indirect RAM)
		MOV	code_buf_ptr,#code_buffer+1		;Initialize code_buf_ptr to next avail open space
		MOV	code_buf_cntr,#0x01	; Counter starts out at 1 (since just put "$" into position 0
		SETB	code_buff		; We want future characters to go into the code buffer
		CLR	ignore			; We would like to receive characters
		JMP	return			; Return from interrupt


no_start_chr:	JB	ignore,return		; If ignore bit is set, just return from interrups
		JB	code_buff,load_code	; If code bit is set, put it in the code buffer
						; Code_buff flag not set (so put in char buffer)

		MOV	R1,char_buf_cntr	; Get the number of chars we've gotten so far
		CJNE	R1,#0x5A,load_char	; If buffer not full (ie = 90), load char buffer
						; Buffer is full!
		SETB	ignore
		SETB	overflow
		JMP	return


load_char:	MOV	R0,char_buf_ptr		; Get current value of the char pointer
		MOV	@R0,A			; Save char in the buffer
		INC	char_buf_ptr
		INC	char_buf_cntr
		JMP	return


load_code:	MOV	R0,code_buf_ptr		; Get location of where to store character
		MOV	@R0,A			; Store it in the code buffer
		INC	code_buf_ptr		; Point to the next open spot
		INC	code_buf_cntr		; Add one to the count
		MOV	R1,code_buf_cntr
		CJNE	R1,#0x06,return		; If code buffer isn't filled, just return
						; Code buffer is full...
		MOV	R0,#code_buffer		; Get a pointer to the start of the code buffer		
		MOV	code_buf_cntr,#0x00	; Initialize counter
loop_1:		MOV	A,@R0			; Get char from code buffer
		MOV	R1,A			; Put it into R1

		MOV	DPTR,#sentence		; Point to start of ASCII message table (the sentence we're looking for)
		MOV	A,code_buf_cntr		; Get current index into table
		MOVC	A,@A+DPTR		; Get character from ROM into A

		CLR	C			; Don't want the carry to interfere (ie subtract w/borrow)
		SUBB	A,R1			; Compare the two characters
		JNZ	bad_sentence		; If different, jump
		MOV	A,code_buf_cntr		; Grab a copy of the counter value
		CLR	C			; Dont want carry to interfere
		SUBB	A,#0x05			; compare with 5
		JZ	good_sentence		; Have we compared all 6 (0 thru 5) characters?  If so, its a good sentence
		INC	R0			; If not, compare more
		INC	code_buf_cntr
		SJMP	loop_1

bad_sentence:	SETB	ignore			; A character was different
		CLR	good_s
		SJMP	return


good_sentence:	SETB	good_s
		MOV	char_buf_cntr,#0x00		; Initialize the char buffer counter
		MOV	char_buf_ptr,#char_buffer	; Point to next open slot (won't contain the $GPXXX)
		CLR	code_buff			; Now we want to save characters in the character buffer
		

		CALL	parse_me			; Parse the buffer into the ASCII raw_xxxx vars
		CALL	convert_me			; Converts the parsed ASCII values (raw_xxxx) to integers (c_xxxx)
return:		CALL	decide_compass			; Decide whether to use comapss or GPS, depending on speed

		CLR	RI				; Clear the interrupt bit
	RET
; END OF serial_isr


;--------------------------------------------------------------------------------------------------------
; 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



;-----------------------------------------------------------------------------
; (serial) Subroutine to parse the GPS sentence into the raw_xxxxx vars
;-----------------------------------------------------------------------------
parse_me:	;initialize pointers here
		MOV	R0,#char_buffer		; Get pointer to the start of character buffer
		MOV	R1,#raw_latitude	; Start with pointer to raw_latitude
		MOV	parse_key_cntr,#0x00	; Initialize the counter


parse_loop:	; Get the next char from parse_key that we're looking for into R2
		MOV	DPTR,#parse_key		; Point to start of parse_key table
		MOV	A,parse_key_cntr	; Get current index into table
		MOVC	A,@A+DPTR		; Get character from parse_key into A
		MOV	R2,A			; Transfer into R2 & do assy version of a CASE statement...

;Case ","
		CJNE	R2,#',',try1		; Do "skip to next comma" routine here
		MOV	A,@R0			; Get character from char buffer
nxt_char2:	CJNE	A,#',',nxt_char1	; Is it a comma?
		INC	R0			; If so, then point to next char &
		INC	parse_key_cntr		; increment parse key counter &
		SJMP	parse_loop		; loop back to top

nxt_char1:	INC	R0			; Point to next character in char buffer
		MOV	A,@R0			; Get next character from char buffer		
		SJMP	nxt_char2


;Case "S"
try1:		CJNE	R2,#'S',try2		; Do skip single character routine here
		INC	R0			; Increment char buffer pointer
		INC	parse_key_cntr		; Advance to next parse key char
		SJMP	parse_loop


;Case "R"
try2:		CJNE	R2,#'R',try3		; Do record character here
		MOV	A,@R0			; Get character from char buffer
		MOV	@R1,A			; Put the character in the next open slot
		INC	parse_key_cntr		; Point to next parse_key char
		INC	R0			; Increment char buffer pointer
		INC	R1			; Point to next slot in raw_xxxx variable
		SJMP	parse_loop


;Case "E"
try3:		CJNE	R2,#'E',try_bad		; Do end routine here
			
		RET	;Just return

try_bad:	;Do what needs to be done if we read a bad character from the parse_key table
		RET	; For now, just return


;-----------------------------------------------------------------------------
; (serial) Subroutine to convert ASCII characters from raw_xxxx into the c_xxxx vars we want
;-----------------------------------------------------------------------------
convert_me:	; Do we need to push any registers?
;Convert "raw_heading" first
		MOV	R0,#raw_heading		;Point at heading
		CALL	conv_3_digits		;Leaves result in temp_sum_l
		;Now shift 16 bit sum right once (divide by 2)
		MOV	A,temp_sum_h		; (Initial value of carry doesn't matter)
		RRC	A			; Get the LSB of temp_sum_h into carry flag
		MOV	A,temp_sum_l
		RRC	A			; Shift right once, bringing carry into MSB
		MOV	R0,#c_heading		;Point at c_heading
		MOV	A,temp_sum_l		;Grab temporary result and...
		MOV	@R0,A			; Put it in c_heading


;Convert "raw_speed" next
		MOV	R0,#raw_speed		;Point at speed
		CALL	conv_3_digits		;Leaves result in temp_sum_l
		MOV	R0,#c_speed		;Point at c_speed
		MOV	A,temp_sum_l		;Grab temporary result and...
		MOV	@R0,A			; Put it in c_heading


;Now, convert "raw_latitude"
		MOV	R0,#raw_latitude	;Point at latitude
		CALL	conv_5_digits		;Leaves result in temp_sum_l & temp_sum_h
		MOV	R0,#c_lat		;Point at high byte of c-lat
		MOV	A,temp_sum_h		;Grab temporary result and...
		MOV	@R0,A			; Put it in c_lat high byte
		INC	R0			;Point at low byte of c_lat
		MOV	A,temp_sum_l		;Grab temporary result and...
		MOV	@R0,A			; Put it in c-lat low byte

;Now, "convert raw_longitude"
		MOV	R0,#raw_longitude	;Point at longitude
		CALL	conv_5_digits		;Leaves result in temp_sum_l & temp_sum_h
		MOV	R0,#c_lon		;Point at high byte of c-lon
		MOV	A,temp_sum_h		;Grab temporary result and...
		MOV	@R0,A			; Put it in c_lon high byte
		INC	R0			;Point at low byte of c_lon
		MOV	A,temp_sum_l		;Grab temporary result and...
		MOV	@R0,A			; Put it in c-lon low byte

		RET



; Subroutine to convert ASCII digits into 2-byte integer.  Two entry points:
; conv_5_digits & conv_3_digits.
; R0 is a pointer assumed to point to the start of the raw_xxx ASCII text to convert.
; Routine will return with temp_sum_l & temp_sum_h filled.
;
conv_5_digits:
		MOV	temp_sum_l,#0x00
		MOV	temp_sum_h,#0x00
		CALL	get_ascii

ten_thou:	DJNZ	R2,do_it_tt
		JMP	thousands
do_it_tt:	MOV	A,temp_sum_l
		ADD	A,#0x10			; Add low byte of "10,000"
		MOV	temp_sum_l,A
		MOV	A,temp_sum_h
		ADDC	A,#0x27			; Add high byte of "10,000"
		SJMP	ten_thou

thousands:	CALL	get_ascii
thou:		DJNZ	R2,do_it_t
		JMP	hundreds
do_it_t:	MOV	A,temp_sum_l
		ADD	A,#0xE8			; Add low byte of "1,000"
		MOV	temp_sum_l,A
		MOV	A,temp_sum_h
		ADDC	A,#0x03			; Add high byte of "1,000"
		SJMP	thou
conv_3_digits:
hundreds:	CALL	get_ascii
hund:		DJNZ	R2,do_it_h
		JMP	tens
do_it_h:	MOV	A,temp_sum_l
		ADD	A,#0x64			; Add low byte of "100"
		MOV	temp_sum_l,A
		MOV	A,temp_sum_h
		ADDC	A,#0x00			; Add high byte of "100"
		SJMP	hund


tens:		CALL	get_ascii
ten:		DJNZ	R2,do_it_x
		JMP	ones
do_it_x:	MOV	A,temp_sum_l
		ADD	A,#0x0A			; Add low byte of "10"
		MOV	temp_sum_l,A
		MOV	A,temp_sum_h
		ADDC	A,#0x00			; Add high byte of "10"
		SJMP	ten

ones:		CALL	get_ascii
		MOV	A,temp_sum_l
		DEC	R2			; Adjust R2 so that get_ascii returns proper value
		ADD	A,R2			; Add R2 into
		MOV	temp_sum_l,A
		MOV	A,temp_sum_h
		ADDC	A,#0x00			; Add high byte
		
		RET


;------------------------------------------------------------
get_ascii:
		MOV	A, @R0
		CLR	C
		SUBB	A,#0x2F
		MOV	R2, A
		INC	R0
		RET

; END OF serial_interrupt routine


;======================================================================================================

; !!! now void / void !!!!
; /* GET_WAYPOINT : blocks for navigator to send new waypoint. */
; void asm_get_waypoint(void) {

	RSEG  ?PR?asm_get_waypoint?COMMON
asm_get_waypoint:
	USING	0

		MOV	R2,#0x20		; Counter (load w/32) (Matt's "temp")
ipc_wait:	JNB	IPC_ACK,ipc_wait
		
		; Make ipc_data equal what is actually on the data line
		CLR	ipc_data
		JNB	IPC_DAT,data_was_clear
		SETB	ipc_data

data_was_clear:	SETB	IPC_CLK

ipc_wait2:	JB	IPC_ACK,ipc_wait2

		CLR	IPC_CLK			

		; Bit received, shift it into register...
		CLR	C
		JNB	ipc_data,ipc_clear
		SETB	C
ipc_clear:	
		MOV	R0,#d_lon		; Get pointer d_lon
	
		MOV	A,@R0
		RLC	A
		MOV	@R0,A
		INC	R0
		MOV	A,@R0
		RLC	A
		MOV	@R0,A
		INC	R0
		MOV	A,@R0
		RLC	A
		MOV	@R0,A
		INC	R0
		MOV	A,@R0
		RLC	A
		MOV	@R0,A
		INC	R0

		DEC	R2		;Decrement Matt's temp

		CJNE	R2,#0x00,ipc_wait
	

?C0003:
	RET  	
; END OF asm_get_waypoint


;======================================================================================================

; !!! now void / void !!!!
; /* SEND_WAYPOINT : Sends current location to navigator
; unsigned char the_data <- A pointer to the pos struct*/
; void asm_send_waypoint(unsigned char the_data) {

	RSEG  ?PR?asm_send_waypoint?COMMON
asm_send_waypoint:
		USING	0

		MOV	R2,#0x20		; Counter (load w/32) (Matt's "temp")
ipc_wait12:	JB	IPC_ACK,ipc_wait12

snd_nxt_bit:	DEC	R2

		MOV	R0,#c_lon		; Get pointer d_lon
	
		CLR	C
		MOV	A,@R0
		RLC	A
		MOV	@R0,A
		INC	R0
		MOV	A,@R0
		RLC	A
		MOV	@R0,A
		INC	R0
		MOV	A,@R0
		RLC	A
		MOV	@R0,A
		INC	R0
		MOV	A,@R0
		RLC	A
		MOV	@R0,A
		INC	R0	

		; At this point, C has MSB of c_lat
		JNC	or_a_zero
		MOV	R0,#c_lon
		INC	@R0			; Set the LSB, since we know it was a zero
				
or_a_zero:	JB	IPC_ACK,or_a_zero	;Make sure ACK is clear before proceeding

		JC	data_line_set
		CLR	IPC_DAT

data_line_set:	; Make ipc_data equal what is actually on the data line
		SETB	IPC_CLK

ipc_wait13:	JNB	IPC_ACK,ipc_wait13	;Wait for ACK
		SETB	IPC_DAT	
		CLR	IPC_CLK

		CJNE	R2,#0x00,ipc_wait12

?C0004:
	RET  	
; END OF _asm_send_waypoint


;======================================================================================================


; /* Initialize everything EXCEPT THE STACK! */
; void asm_init(void) {
;
; NOTE:  This routine hangs until at least one good GPS sentence is received!
	RSEG  ?PR?asm_init?COMMON
asm_init:

		; Make sure IPC_DAT pin is an input
		SETB	IPC_DAT
		CLR	IPC_CLK

		; Initialization for Servo routine
		CLR	YGM			; Clear the output lines
		CLR	DAT

		MOV	command_byte,#0x00
		MOV	servo_data,#0x00
		MOV	bit_count,#0x11		; Must be 1 more than the number of bits to send (ie decimal 17)

		MOV	R1,#0x80		; Wait about 2 seconds for Motorola chip to stabilize
		CALL	long_delay
		; End of Initialization for Servo routine


		; Initialization for LCD routine:
		CLR	e_line
		CLR	rs
		CLR	P1_4
		CLR	P1_5
		CLR	P1_6
		CLR	P1_7

		; Initialize the LCD Display
		; Wait for 15ms
		MOV	R1,#150		; Set delay time
		CALL	var_delay

		; Send init command
		MOV	lcd_data,#0x30
		SETB	e_line
		NOP			; Min cycle time is 500ns
		CLR	e_line

		; Wait for 4.1ms
		MOV	R1,#41
		CALL	var_delay

		; Send init command
		MOV	lcd_data,#0x30
		SETB	e_line
		NOP			; Min cycle time is 500ns
		CLR	e_line

		; Wait for 100us
		MOV	R1,1
		CALL	var_delay

		; Send init command
		MOV	A,#0x30
		CALL	lcd_write

		; Send function set command
		; 4-bit bus, 2 rows, 5x7 dots
		MOV	A,#0x20
		CALL	lcd_write
		MOV	A,#0x20
		CALL	lcd_write
		MOV	A,#0x80
		CALL	lcd_write

		; Send display control command
		; Display on, cursor off, no blinking
		MOV	A,#0x00
		CALL	lcd_write
		MOV	A,#0xC0
		CALL	lcd_write

		; Send clear display command
		; Clear display, Cursor address = 0
		MOV	A,#0x00
		CALL	lcd_write
		MOV	R1,#16
		CALL	var_delay
		MOV	A,#0x10
		CALL	lcd_write
		MOV	R1,#16
		CALL	var_delay

		; Send entry mode command
		; Increment, no display shift
		MOV	A,0x00
		CALL	lcd_write
		MOV	A,#0x60
		CALL	lcd_write
; End of initialization for LCD Display Routine




; Initialize the serial port
		MOV	RCAP2H,#0xFF
		MOV	RCAP2L,#0x30	; Set up timer for 4800 baud rate

		MOV	code_buf_ptr,#code_buffer	; Initialize pointer to first slot in code_buffer
		MOV	char_buf_ptr,#char_buffer	; Initialize pointer to first slot in char_buffer

		MOV	code_buf_cntr,#0x00		; Counts number of characters in code buffer
		MOV	char_buf_cntr,#0x00		; Counts number of characters in caracter buffer

		SETB	ignore
		CLR	good_s
		CLR	s_avail


		MOV	T2MOD,#0x02	; Set up T2MOD to enable timer 2 output
		MOV	T2CON,#0x34	; Set up timer 2 and start it. (RCLK for T2, TCKL for T2, turn on T2) 

		MOV	IE,#0x90	; Disable timer 2 interrupts, enable serial port interrupts
		MOV	SCON,#0x70	; Set up serial control register, & enable it.

		
s_pause:	NOP			; Hang until we get at least one good GPS sentence in.
;		JNB	s_avail,s_pause	; If the sentence isn't good yet, then just waste time	
		
; End of initialization of the serial port



?C0005:
		RET  	
; END OF asm_init



;======================================================================================================


; // LCD_PRINT
; //  Passes a pointer (in R7) to a null-terminated string the C code would like ASM to print
; void	asm_lcd_print(unsigned char data_ptr) {

		RSEG  ?PR?asm_lcd_print?COMMON
asm_lcd_print:
		
		;MOV	data_ptr?643,R7		;For some reason, it doesn't like: MOV	R1,R7
		MOV	R1,#message
		
		; Assumes pointer to null-terminated string to display is in R1
		CALL	show_em		; Update the LCD display
?C0007:
		RET  	

;--------------------------------------------------
; LCD Subroutines
;--------------------------------------------------

;var_delay routine.
;--------------------------
; To use:  Load variable R1 with number of tenths of milliseconds you want to delay,
; then call this subroutine.  Routine will return after specified amount of time.
; Min. delay time (R1 = 1) is 100us, max. delay time (R1 = 0) is 25.6ms.
; Assumes a 32MHz crystal, for an internal cycle time of 375ns (internal divide-by-12)
; Formula is delay = R1 * 100us (if R1 = 1, get a 100us delay, if R1 = 10, get a 1ms delay)
; The timing is close, but not exact.  See following chart:
; R1 = 1, Delay Error = +0.875us (100.875us)
; R1 = 10, Delay Error = +1.25us (1001.25us)
; R1 = 100, Delay Error = +13.3us (10.0133ms)
; R1 = 0, Delay Error = +32.75us (25.63275ms)
; Number of cycles for each instruction shown in comments
		
var_delay:	MOV	R0,#132		;1 = 375ns
L1:		DJNZ	R0,L1		;2 = 750ns (do this 132 times)
		DJNZ	R1,var_delay 	;2 = 750ns (do this R1 times)
		RET			;2 = 750ns

		;lcd_write routine - Sends data to the LCD
lcd_write:	MOV	lcd_data,A	; Upper 4 bits connect to LCD
		SETB	e_line		; Toggle enable line
		NOP			; 500ns minimum cycle time
		CLR	e_line
		MOV	R0,#53		;Simple 40us delay loop for LCD
L2:		DJNZ	R0,L2		;
		RET

		;lcd_addr - Routine to send LCD address for start of characters
lcd_addr:	CLR	rs		; Put LCD in command mode
		MOV	lcd_data,A
		SETB	e_line		; Clock in the data
		NOP			; 500ns minimum cycle time
		CLR	e_line
		MOV	R0,#53		;Simple 40us delay loop for LCD
L4:		DJNZ	R0,L4		;
		SETB	rs		; Put LCD in data mode
		RET

		; Assumes that R1 points to the null terminated string to display
show_em:	MOV 	A,#0x80		; Addr = 80H MSB (remember, we send 4 bits at a time!)
		CALL	lcd_addr	; (See Optrex spec sheet for address format)
		MOV	A,#0x00		; Addr = 80H LSB
		CALL	lcd_addr

L3:		MOV	A,@R1		; Get character (for high nibble)
		JZ	outmsg		; Are we done displaying message?
		CALL	lcd_write
		ANL	A,#0x0F		; Mask high nibble, so rotate works like shift
		RL	A
		RL	A
		RL	A
		RL	A
		CALL	lcd_write
		INC	R1		; Increment index to point to next character
		SJMP	L3
outmsg:		RET		
; END OF asm_lcd_print


;------------------------------------------------------------------------------
; The END directive is ALWAYS required.
;------------------------------------------------------------------------------
		END			; End of everything