.title "GPS Speedometer"
	.sbttl "Using the PIC24"
	.equ __24FJ64GA002, 1
	.include "p24FJ64GA002.inc"

; DB<7:0> --> PB<15:8> PA<15:8>
; CS1 --> PA0 PG0
; CS2 --> PA1 PG1
; RST --> PA2 nc
; R/W --> GND RG12
; D/I --> PA3 RG13
; E   --> PA4 RG14
; 
; RP5 (pin14) UART1 RX --> GPS TXD
; RP4 (pin12) UART1 TX --> GPS RXD
;
; PGD1/RP0 (pin4) UART2 TX --> PICkit2 pin4 PGD/RX 
; PGC1/RP1 (pin5) UART2 RX --> PICkit2 pin5 PGC/TX 

	.global __reset          ;The label for the first line of code. 
;	.global __T1Interrupt    ;Declare Timer 1 ISR name global
;	.global	__CNInterrupt

.bss
;	onesecond:	.space 2
;	cnt:		.space 2
	dspbuff:	.space 1024
	msgbuff:	.space 72
	s1:			.space 2
	s10:		.space 2
	m1:			.space 2
	m10:		.space 2
	h1:			.space 2
	h10:		.space 2
	tts1:		.space 2
	tts10:		.space 2
	ttm1:		.space 2
	ttm10:		.space 2
	tth1:		.space 2
	tth10:		.space 2
	dd1:		.space 2
	dd10:		.space 2
	mm1:		.space 2
	mm10:		.space 2
	yy1:		.space 2
	yy10:		.space 2
	tts:		.space 2
	ttm:		.space 2
	tth:		.space 2
	midnight:	.space 2
.section .const,psv
d0:	.byte 0xc0,0x3f,0x00,0xe0,0xff,0x00,0xf8,0xff,0x01,0xfc,0xff,0x03,0xfc,0xff,0x07
 	.byte 0x7e,0xe0,0x07,0x3f,0xc0,0x0f,0x1f,0x80,0x0f,0x1f,0x80,0x0f,0x1f,0x80,0x0f
  	.byte 0x1f,0x80,0x0f,0x1f,0x80,0x0f,0x1f,0x80,0x0f,0x1f,0x80,0x0f,0x1f,0x80,0x0f
 	.byte 0x1f,0x80,0x0f,0x1f,0x80,0x0f,0x1f,0x80,0x0f,0x1f,0x80,0x0f,0x1f,0x80,0x0f
	.byte 0x1f,0x80,0x0f,0x1f,0x80,0x0f,0x1f,0x80,0x0f,0x1f,0x80,0x0f,0x1f,0x80,0x0f
 	.byte 0x1f,0x80,0x0f,0x1f,0x80,0x0f,0x1f,0x80,0x0f,0x3f,0xc0,0x0f,0x7e,0xe0,0x07
	.byte 0xfe,0xff,0x07,0xfc,0xff,0x03,0xf8,0xff,0x01,0xf0,0xff,0x00,0xc0,0x3f,0x00
d1: .byte 0x80,0x1f,0x00,0x80,0x3f,0x00,0x80,0x3f,0x00,0x80,0x7f,0x00,0x80,0x7f,0x00
  	.byte 0x80,0xff,0x00,0x80,0xff,0x00,0x80,0xff,0x01,0x80,0xff,0x01,0x80,0xff,0x03
	.byte 0x80,0xff,0x03,0x80,0xff,0x07,0x80,0xff,0x07,0x80,0xdf,0x0f,0x80,0xdf,0x0f
  	.byte 0x80,0x1f,0x00,0x80,0x1f,0x00,0x80,0x1f,0x00,0x80,0x1f,0x00,0x80,0x1f,0x00
  	.byte 0x80,0x1f,0x00,0x80,0x1f,0x00,0x80,0x1f,0x00,0x80,0x1f,0x00,0x80,0x1f,0x00
  	.byte 0x80,0x1f,0x00,0x80,0x1f,0x00,0x80,0x1f,0x00,0x80,0x1f,0x00,0x80,0x1f,0x00
  	.byte 0xff,0xff,0x0f,0xff,0xff,0x0f,0xff,0xff,0x0f,0xff,0xff,0x0f,0xff,0xff,0x0f
d2:	.byte 0xc0,0x3f,0x00,0xf0,0xff,0x00,0xf8,0xff,0x01,0xfc,0xff,0x03,0xfe,0xff,0x07
   	.byte 0x7e,0xe0,0x07,0x3f,0xc0,0x0f,0x1f,0x80,0x0f,0x1f,0x80,0x0f,0x1f,0x80,0x0f
   	.byte 0x1f,0x00,0x00,0x1f,0x00,0x00,0x3f,0x00,0x00,0x7f,0x00,0x00,0xfe,0x00,0x00
   	.byte 0xfe,0x01,0x00,0xfc,0x03,0x00,0xf8,0x07,0x00,0xf0,0x07,0x00,0xe0,0x0f,0x00
   	.byte 0xc0,0x1f,0x00,0xc0,0x3f,0x00,0x80,0x7f,0x00,0x00,0x7f,0x00,0x00,0xfe,0x00
   	.byte 0x00,0xfc,0x01,0x00,0xfc,0x03,0x00,0xf8,0x07,0x00,0xf0,0x0f,0x00,0xe0,0x0f
   	.byte 0xff,0xff,0x0f,0xff,0xff,0x0f,0xff,0xff,0x0f,0xff,0xff,0x0f,0xff,0xff,0x0f
d3: .byte 0xc0,0x3f,0x00,0xf0,0xff,0x00,0xf8,0xff,0x01,0xfc,0xff,0x03,0xfe,0xff,0x07
   	.byte 0x7e,0xe0,0x07,0x3f,0xc0,0x0f,0x1f,0x80,0x0f,0x1f,0x80,0x0f,0x1f,0x80,0x0f
   	.byte 0x1f,0x00,0x00,0x1f,0x00,0x00,0x1f,0x00,0x00,0x3f,0x00,0x00,0x7e,0x00,0x00
   	.byte 0xfe,0x03,0x00,0xfc,0x03,0x00,0xf8,0x03,0x00,0xfc,0x03,0x00,0xfe,0x03,0x00
   	.byte 0x7e,0x00,0x00,0x3f,0x00,0x00,0x1f,0x00,0x00,0x1f,0x00,0x00,0x1f,0x00,0x00
   	.byte 0x1f,0x80,0x0f,0x1f,0x80,0x0f,0x1f,0x80,0x0f,0x3f,0xc0,0x0f,0x7e,0xe0,0x07
   	.byte 0xfe,0xff,0x07,0xfc,0xff,0x03,0xf8,0xff,0x01,0xf0,0xff,0x00,0xc0,0x3f,0x00
d4:	.byte 0x00,0xfc,0x00,0x00,0xfc,0x00,0x00,0xfc,0x00,0x00,0xfc,0x00,0x00,0xf8,0x00
   	.byte 0x00,0xf8,0x01,0x00,0xf8,0x01,0x00,0xf8,0x01,0x00,0xf8,0x01,0x00,0xf0,0x01
   	.byte 0x00,0xf0,0x03,0x00,0xf0,0x03,0x00,0xf0,0x03,0x00,0xf0,0x03,0x00,0xe0,0x03
   	.byte 0xe0,0xe3,0x07,0xe0,0xe3,0x07,0xe0,0xe3,0x07,0xe0,0xe3,0x07,0xe0,0xc3,0x07
   	.byte 0xff,0xff,0x0f,0xff,0xff,0x0f,0xff,0xff,0x0f,0xff,0xff,0x0f,0xff,0xff,0x0f
   	.byte 0xe0,0x03,0x00,0xe0,0x03,0x00,0xe0,0x03,0x00,0xe0,0x03,0x00,0xe0,0x03,0x00
   	.byte 0xe0,0x03,0x00,0xe0,0x03,0x00,0xe0,0x03,0x00,0xe0,0x03,0x00,0xe0,0x03,0x00
d5: .byte 0xff,0xff,0x0f,0xff,0xff,0x0f,0xff,0xff,0x0f,0xff,0xff,0x0f,0xff,0xff,0x0f
   	.byte 0x00,0x80,0x0f,0x00,0x80,0x0f,0x00,0x80,0x0f,0x00,0x80,0x0f,0x00,0x80,0x0f
   	.byte 0xc0,0xbf,0x0f,0xe0,0xff,0x0f,0xf8,0xff,0x0f,0xfc,0xff,0x0f,0xfc,0xff,0x0f
   	.byte 0x7e,0xe0,0x0f,0x3f,0xc0,0x0f,0x1f,0x80,0x0f,0x1f,0x80,0x0f,0x1f,0x80,0x0f
   	.byte 0x1f,0x00,0x00,0x1f,0x00,0x00,0x1f,0x00,0x00,0x1f,0x00,0x00,0x1f,0x00,0x00
   	.byte 0x1f,0x80,0x0f,0x1f,0x80,0x0f,0x1f,0x80,0x0f,0x3f,0xc0,0x0f,0x7e,0xe0,0x07
   	.byte 0xfe,0xff,0x07,0xfc,0xff,0x03,0xf8,0xff,0x01,0xf0,0xff,0x00,0xc0,0x3f,0x00
d6:	.byte 0xc0,0x3f,0x00,0xe0,0xff,0x00,0xf8,0xff,0x01,0xfc,0xff,0x03,0xfc,0xff,0x07
   	.byte 0x7e,0xe0,0x07,0x3f,0xc0,0x0f,0x1f,0x80,0x0f,0x1f,0x80,0x0f,0x1f,0x80,0x0f
   	.byte 0x00,0x80,0x0f,0x00,0x80,0x0f,0xc0,0xbf,0x0f,0xe0,0xff,0x0f,0xf8,0xff,0x0f
   	.byte 0xfc,0xff,0x0f,0xfc,0xff,0x0f,0x7e,0xe0,0x0f,0x3f,0xc0,0x0f,0x1f,0x80,0x0f
   	.byte 0x1f,0x80,0x0f,0x1f,0x80,0x0f,0x1f,0x80,0x0f,0x1f,0x80,0x0f,0x1f,0x80,0x0f
   	.byte 0x1f,0x80,0x0f,0x1f,0x80,0x0f,0x1f,0x80,0x0f,0x3f,0xc0,0x0f,0x7e,0xe0,0x07
   	.byte 0xfe,0xff,0x07,0xfc,0xff,0x03,0xf8,0xff,0x01,0xf0,0xff,0x00,0xc0,0x3f,0x00
d7: .byte 0xff,0xff,0x0f,0xff,0xff,0x0f,0xff,0xff,0x0f,0xff,0xff,0x0f,0xff,0xff,0x0f
   	.byte 0x3f,0x00,0x00,0x7f,0x00,0x00,0x7e,0x00,0x00,0xfe,0x00,0x00,0xfc,0x00,0x00
   	.byte 0xfc,0x01,0x00,0xf8,0x01,0x00,0xf8,0x03,0x00,0xf0,0x03,0x00,0xf0,0x07,0x00
   	.byte 0xe0,0x07,0x00,0xe0,0x0f,0x00,0xc0,0x0f,0x00,0xc0,0x1f,0x00,0x80,0x1f,0x00
   	.byte 0x80,0x3f,0x00,0x00,0x3f,0x00,0x00,0x7f,0x00,0x00,0x7e,0x00,0x00,0xfe,0x00
   	.byte 0x00,0xfc,0x00,0x00,0xfc,0x01,0x00,0xf8,0x01,0x00,0xf8,0x01,0x00,0xf8,0x03
   	.byte 0x00,0xf0,0x03,0x00,0xf0,0x07,0x00,0xe0,0x07,0x00,0xe0,0x0f,0x00,0xc0,0x0f
d8: .byte 0xc0,0x3f,0x00,0xf0,0xff,0x00,0xf8,0xff,0x01,0xfc,0xff,0x03,0xfe,0xff,0x07
   	.byte 0x7e,0xe0,0x07,0x3f,0xc0,0x0f,0x1f,0x80,0x0f,0x1f,0x80,0x0f,0x1f,0x80,0x0f
   	.byte 0x1f,0x80,0x0f,0x1f,0x80,0x0f,0x1f,0x80,0x0f,0x3f,0xc0,0x0f,0x7e,0xe0,0x07
   	.byte 0xfe,0xff,0x07,0xfc,0xff,0x03,0xf8,0xff,0x01,0xfc,0xff,0x03,0xfe,0xff,0x07
   	.byte 0x7e,0xe0,0x07,0x3f,0xc0,0x0f,0x1f,0x80,0x0f,0x1f,0x80,0x0f,0x1f,0x80,0x0f
   	.byte 0x1f,0x80,0x0f,0x1f,0x80,0x0f,0x1f,0x80,0x0f,0x3f,0xc0,0x0f,0x7e,0xe0,0x07
   	.byte 0xfe,0xff,0x07,0xfc,0xff,0x03,0xf8,0xff,0x01,0xf0,0xff,0x00,0xc0,0x3f,0x00
d9:	.byte 0xc0,0x3f,0x00,0xe0,0xff,0x00,0xf8,0xff,0x01,0xfc,0xff,0x03,0xfc,0xff,0x07
   	.byte 0x7e,0xe0,0x07,0x3f,0xc0,0x0f,0x1f,0x80,0x0f,0x1f,0x80,0x0f,0x1f,0x80,0x0f
   	.byte 0x1f,0x80,0x0f,0x1f,0x80,0x0f,0x1f,0x80,0x0f,0x1f,0x80,0x0f,0x1f,0x80,0x0f
   	.byte 0x1f,0x80,0x0f,0x3f,0xc0,0x0f,0x7f,0xe0,0x07,0xff,0xff,0x07,0xff,0xff,0x03
   	.byte 0xff,0xff,0x01,0xff,0xff,0x00,0xdf,0x3f,0x00,0x1f,0x00,0x00,0x1f,0x00,0x00
   	.byte 0x1f,0x80,0x0f,0x1f,0x80,0x0f,0x1f,0x80,0x0f,0x3f,0xc0,0x0f,0x7e,0xe0,0x07
   	.byte 0xfe,0xff,0x07,0xfc,0xff,0x03,0xf8,0xff,0x01,0xf0,0xff,0x00,0xc0,0x3f,0x00
d19:.byte 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00
   	.byte 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00
   	.byte 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00
   	.byte 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00
   	.byte 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00
   	.byte 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00
   	.byte 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00
	.byte 0x00	; even address boundary
sd0: .byte 0x3C, 0x7E, 0x66, 0x66, 0x66, 0x66, 0x66, 0x66, 0x7E, 0x3C
sd1: .byte 0x18, 0x38, 0x78, 0x18, 0x18, 0x18, 0x18, 0x18, 0x7E, 0x7E
sd2: .byte 0x3C, 0x7E, 0x66, 0x66, 0x0C, 0x18, 0x30, 0x60, 0x7E, 0x7E
sd3: .byte 0x3C, 0x7E, 0x66, 0x06, 0x1C, 0x1C, 0x06, 0x66, 0x7E, 0x3C
sd4: .byte 0x18, 0x18, 0x30, 0x30, 0x6C, 0x6C, 0x7E, 0x7E, 0x0C, 0x0C
sd5: .byte 0x7E, 0x7E, 0x60, 0x7C, 0x7E, 0x66, 0x06, 0x66, 0x7E, 0x3C
sd6: .byte 0x3C, 0x7E, 0x66, 0x60, 0x7C, 0x7E, 0x66, 0x66, 0x7E, 0x3C
sd7: .byte 0x7E, 0x7E, 0x06, 0x06, 0x0C, 0x0C, 0x18, 0x18, 0x30, 0x30
sd8: .byte 0x3C, 0x7E, 0x66, 0x66, 0x7E, 0x7E, 0x66, 0x66, 0x7E, 0x3C
sd9: .byte 0x3C, 0x7E, 0x66, 0x66, 0x7E, 0x3E, 0x06, 0x66, 0x7E, 0x3C
sdc: .byte 0x00, 0x00, 0x18, 0x18, 0x00, 0x00, 0x18, 0x18, 0x00, 0x00
sds: .byte 0x06, 0x06, 0x0C, 0x0C, 0x18, 0x18, 0x30, 0x30, 0x60, 0x60

;msg_length: .byte 11, 11, 11, 11, 11, 11, 11
RMC:		.byte 0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x04, 0x00, 0xFE, 0x17
GGA: 		.byte 0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x00, 0x00, 0xFA, 0x0F
GSA: 		.byte 0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x02, 0x00, 0xFC, 0x13
GSV: 		.byte 0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x03, 0x00, 0xFD, 0x15
GLL: 		.byte 0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x01, 0x00, 0xFB, 0x11
VTG: 		.byte 0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x05, 0x00, 0xFF, 0x19
TPpol:		.byte 0xB5, 0x62, 0x06, 0x07, 0x14, 0x00, 0x40, 0x42, 0x0f, 0x00, 0xA0, 0x86, 0x01, 0x00, 0xFF, 0x01, 0x00, 0x00, 0x32, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0B, 0x54
Automotive:	.byte 0xB5, 0x62, 0x06, 0x24, 0x24, 0x00, 0x01, 0x00, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x53, 0x70
TIMEUTC:	.byte 0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0x01, 0x21, 0x01, 0x2D, 0x85
VELNED:		.byte 0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0x01, 0x12, 0x01, 0x1E, 0x67
ack:		.byte 0xB5, 0x62, 0x05, 0x01, 0x02, 0x00  ; 0x06, 0x01, 0x0F, 0x38
vel:		.byte 0xB5, 0x62, 0x01, 0x12, 0x24, 0x00
d_in_m:		.byte 31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31	; ordinary year
d_in_ml:	.byte 31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31	; leap year

.text                             ;Start of Code section
__reset:
	mov 	#__SP_init, W15		; Initalize the Stack Pointer
	mov 	#__SPLIM_init, W0	; Initialize the Stack Pointer Limit Register
	mov 	W0, SPLIM
	nop							; Add NOP to follow SPLIM initialization
 
	call	init_PSV
;	call	init_timer

;	bset	AD1PCFG, #PCFG0
;	bset	AD1PCFG, #PCFG1
	mov		#0xFFFF, W0
	mov		W0, AD1PCFG

	mov		#0x0000, W0
	mov		W0, LATA
	mov		W0, LATB

	mov		#0xFFE0, W0
	mov		W0, TRISA
	mov		#0x00FF, W0
	mov		W0, TRISB


	bclr	RPINR18, #U1RXR4
	bclr	RPINR18, #U1RXR3
	bset	RPINR18, #U1RXR2
	bclr	RPINR18, #U1RXR1
	bset	RPINR18, #U1RXR0	; RP5 UART1 RX
	;
	bclr	RPOR2, #RP4R4
	bclr	RPOR2, #RP4R3
	bclr	RPOR2, #RP4R2
	bset	RPOR2, #RP4R1
	bset	RPOR2, #RP4R0		; U1TX Output Function #3 mapped to RP4

	bclr	RPINR19, #U2RXR4
	bclr	RPINR19, #U2RXR3
	bclr	RPINR19, #U2RXR2
	bclr	RPINR19, #U2RXR1
	bset	RPINR19, #U2RXR0	; RP1 UART2 RX
	;
	bclr	RPOR0, #RP0R4
	bclr	RPOR0, #RP0R3
	bset	RPOR0, #RP0R2
	bclr	RPOR0, #RP0R1
	bset	RPOR0, #RP0R0		; U2TX Output Function #5 mapped to RP0

	bset	U1MODE, #BRGH		; high speed mode
	bset	U2MODE, #BRGH		; high speed mode
	mov		#12, W0
	mov		W0, U1BRG			; 38400 with fcy=2MHz
	mov		W0, U2BRG			; 38400 with fcy=2MHz

	bset	U1MODE, #UARTEN		; UART1 enabled
	bset	U1STA, #UTXEN		; UART1 transmit enable
	bset	U2MODE, #UARTEN		; UART2 enabled
	bset	U2STA, #UTXEN		; UART2 transmit enable


	call	wait1s

	mov		#0, W0
	mov		#s1, W1
	repeat	#20
	mov		W0, [W1++]			; set clock and trip time to 00:00:00, and date to 00/00/00

	mov		#10, W3
	mov		#psvoffset(RMC), W1
next_mode:
	mov.b	[W1+4], W2
	add.b	W2, #8, W2			; # of bytes to send
mode_set_loop:
	btst	U1STA, #UTXBF	; buffer empty? (UTXBF = bit 9)
	bra		NZ, mode_set_loop
	mov.b	[W1++], W0
	mov		W0, U1TXREG
	dec.b	W2, W2
	bra		NZ, mode_set_loop

wait_ack:
	mov		#psvoffset(ack), W6
	mov.b	[W6++], W5
wait_1st_byte:
	call	get_serial_byte
	cp.b	W0, W5
	bra		NZ, wait_1st_byte
	mov		#5, W7
wait_2_6:
	mov.b	[W6++], W5
	call	get_serial_byte
	cp.b	W0, W5
	bra		NZ, wait_ack
	dec		W7, W7
	bra		NZ, wait_2_6
	mov		#4, W7
wait_7_10:
	call	get_serial_byte
	dec		W7, W7
	bra		NZ, wait_7_10


	dec		W3, W3
	bra		NZ, next_mode


	call	delay10us
	bset	LATA, #2
	call	delay10us
	
	bclr	LATA, #0
	bclr	LATA, #1		; both chips selected
	call	delay10us

	mov		#0xC0, W0
	call	send_command
	mov		#0x40, W0
	call	send_command
	mov		#0xB8, W0
	call	send_command
	mov		#0x3F, W0
	call	send_command

	mov		#dspbuff, W1
	mov		#0, W0
	repeat	#511
	mov		W0, [W1++]

;	mov		#3, W10
;	mov		#2, W11
;	mov		#1, W12
;	mov		#0, W2
;	call	set_digit
;	call 	set_time_date
;	call	display
;do_nothing:
;	bra		do_nothing


;tst:
;	btss	U1STA, #URXDA
;	bra		no_rx_1
;	mov		U1RXREG, W0
;	mov		W0, U2TXREG
;no_rx_1:
;
;	btss	U2STA, #URXDA
;	bra		no_rx_2
;	mov		U2RXREG, W0
;	mov		W0, U1TXREG
;no_rx_2:
;	bra		tst


start:
	mov		#msgbuff, W1
	add		W1, #1, W0
	repeat	#70
	mov.b	[W0++], [W1++]
	call	get_serial_byte
	mov.b	W0, [W1]
	mov		#msgbuff, W0
	mov		#psvoffset(vel), W1
	mov		#6, W4
check_vel:
	mov.b	[W0++], W2
	mov.b	[W1++], W3
	cp.b	W2, W3
	bra		NZ, start
	dec		W4, W4
	bra		NZ, check_vel

;	mov		#72, W2
;	mov		#msgbuff, W1
;send_buff:
;	btst	U2STA, #UTXBF
;	bra		NZ, send_buff
;	mov.b	[W1++], W0
;	mov		W0, U2TXREG
;	dec		W2, W2
;	bra		NZ, send_buff
;	bra		start

	mov		#msgbuff, W10
	mov		[W10+22], W2
;	mov		[W10+24], W3		; 3D speed cm/s upper word 0
	mov		#36, W0
	mul.uu	W2, W0, W2
	mov		#1000, W4
	repeat	#17
	div.ud	W2, W4
	mov		#10, W2
	repeat	#17
	div.u	W0, W2
	mov		W1, W10
	cp0		W0
	bra		NZ, tens_not_zero
	mov		#10, W11
	mov		#10, W12
	bra		done_speed
tens_not_zero:
	repeat	#17
	div.u	W0, W2
	mov		W1, W11
	cp0		W0
	bra		NZ, hundreds_not_zero
	mov		#10, W12
	bra		done_speed
hundreds_not_zero:
	mov		W0, W12
done_speed:
	mov		#0, W2
	call	set_digit

	mov		#msgbuff, W10
	mov		[W10+30], W2
	mov		[W10+32], W3		; heading
	mov		#10000, W4
	repeat	#17
	div.ud	W2, W4
	mov		#10, W2
	repeat	#17
	div.u	W0, W2
	repeat	#17
	div.u	W0, W2
	mov		W1, W10
	mov		#10, W2
	repeat	#17
	div.u	W0, W2
	mov		W1, W11
	repeat	#17
	div.u	W0, W2
	mov		W1, W12
	mov		#42, W2
	call	set_digit


	mov		#msgbuff, W10
	mov.b	[W10+69], W0
;	btss	W0, #2
;	bra		time_invalid
	mov		#0, W2
	mov		#10, W3
	mov.b	[W10+68], W2	; get seconds
	repeat	#17
	div.u	W2, W3
	mov		W0, s10
	mov		W1, s1
	mov.b	[W10+67], W2	; get minutes
	repeat	#17
	div.u	W2, W3
	mov		W0, m10
	mov		W1, m1
	mov		#0, W0
	mov		W0, midnight
	mov.b	[W10+66], W2	; get hour
	add		W2, #3, W2		; UTC+3
	cp.b	W2, #24
	bra		LTU, no_hour_correction
	sub		W2, #24, W2
	mov		#1, W0
	mov		W0, midnight
no_hour_correction:
	repeat	#17
	div.u	W2, W3
	mov		W0, h10
	mov		W1, h1
	;
	mov		#0, W4
	mov		#0, W5
	mov.b	[W10+65], W2	; get day of month
	mov.b	[W10+64], W4	; get month
	mov		[W10+62], W5	; get year
	mov		#2000, W0
	sub		W5, W0, W5	
	btst	midnight, #0
	bra		Z, no_date_correction
	mov		#psvoffset(d_in_m), W6
	mov		#0x0003, W7
	and		W5,	W7, W7		; leap year?
	bra		NZ, not_leap_year
	add		W6, #12, W6
not_leap_year:
	mov.b	[W4+W6], W6		; how many days are in this month?
	inc		W2, W2
	cp.b	W2, W6
	bra		LEU, no_date_correction
	mov		#1, W2
	inc		W4, W4
	cp.b	W4, #12
	bra		LEU, no_date_correction
	mov		#1, W4
	inc		W5, W5	
no_date_correction:
	repeat	#17
	div.u	W2, W3
	mov		W0, dd10
	mov		W1, dd1
	repeat	#17
	div.u	W4, W3
	mov		W0, mm10
	mov		W1, mm1
	repeat	#17
	div.u	W5, W3
	mov		W0, yy10
	mov		W1, yy1
	;
	mov		#0x60, W10
	mov		tts, W0
	inc		W0, W0
	daw.b	W0
	mov		W0, tts
	cp.b	W0, W10
	bra		NZ, done_inc_tt
	mov		#0, W0
	mov		W0, tts
	mov		ttm, W0
	inc		W0, W0
	daw.b	W0
	mov		W0, ttm
	cp.b	W0, W10
	bra		NZ, done_inc_tt
	mov		#0, W0
	mov		W0, ttm
	mov		tth, W0
	inc		W0, W0
	daw.b	W0
	mov		W0, tth
done_inc_tt:
	mov		tts, W0
	call	separate_nibbles
	mov		W10, tts1
	mov		W11, tts10
	mov		ttm, W0
	call	separate_nibbles
	mov		W10, ttm1
	mov		W11, ttm10
	mov		tth, W0
	call	separate_nibbles
	mov		W10, tth1
	mov		W11, tth10
	;
	call 	set_time_date
time_invalid:

	call	display
	bra		start


get_serial_byte:
	btss	U1STA, #URXDA
	bra		get_serial_byte
	mov		U1RXREG, W0
	return

display:
	mov		#dspbuff, W5
	mov		#0xB8, W4
	mov		#8, W3
xloop:
	bclr	LATA, #0
	bclr	LATA, #1
	mov		W4, W0
	call	send_command
	bclr	LATA, #0
	bset	LATA, #1
	mov		#2, W2
blockloop:
	mov		#64, W1
yloop:
	mov.b	[W5++], W0
	call	send_data
	dec		W1, W1
	bra		NZ, yloop
	bset	LATA, #0
	bclr	LATA, #1
	dec		W2, W2
	bra		NZ, blockloop
	dec		W3, W3
	bra		Z, done_dsp
	inc		W4, W4
	bra		xloop
done_dsp:
	return


set_digit:
	mov		#105, W0
	mul.uu 	W10, W0, W0
	mov		W0, W10
	mov		#105, W0
	mul.uu 	W11, W0, W0
	mov		W0, W11
	mov		#105, W0
	mul.uu 	W12, W0, W0
	mov		W0, W12
	mov		#psvoffset(d0), W0
	add		W10, W0, W10
	add		W11, W0, W11
	add		W12, W0, W12

	mov		#dspbuff, W0	
	mov		#896, W1
	add		W0, W1, W1
	add		W1, W2, W1
	mov		#35, W13
	mov		W12, W9
b7:
	mov.b	[W9+2], W7
	mov.b	[W9+1], W6
	repeat	#3
	sl.b	W7, W7
	repeat	#3
	lsr.b	W6, W6
	add.b	W7, W6, W0
	mov.b	W0, [W1++]
	add		W9, #3, W9
	dec		W13, W13
	bra		NZ, b7

	mov		#dspbuff, W0	
	mov		#768, W1
	add		W0, W1, W1
	add		W1, W2, W1
	mov		#35, W13
	mov		W12, W9
b6:
	mov.b	[W9+1], W7
	mov.b	[W9+0], W6
	repeat	#3
	sl.b	W7, W7
	repeat	#3
	lsr.b	W6, W6
	add.b	W7, W6, W0
	mov.b	W0, [W1++]
	add		W9, #3, W9
	dec		W13, W13
	bra		NZ, b6

	mov		#dspbuff, W0	
	mov		#640, W1
	add		W0, W1, W1
	add		W1, W2, W1
	mov		#35, W13
	mov		W12, W9
	mov		W11, W8
b5:
	mov.b	[W9+0], W7
	mov.b	[W8+2], W6
	repeat	#3
	sl.b	W7, W7
	repeat	#1
	lsr.b	W6, W6
	add.b	W7, W6, W0
	mov.b	W0, [W1++]
	add		W9, #3, W9
	add		W8, #3, W8
	dec		W13, W13
	bra		NZ, b5

	mov		#dspbuff, W0	
	mov		#512, W1
	add		W0, W1, W1
	add		W1, W2, W1
	mov		#35, W13
	mov		W11, W9
b4:
	mov.b	[W9+2], W7
	mov.b	[W9+1], W6
	repeat	#5
	sl.b	W7, W7
	repeat	#1
	lsr.b	W6, W6
	add.b	W7, W6, W0
	mov.b	W0, [W1++]
	add		W9, #3, W9
	dec		W13, W13
	bra		NZ, b4

	mov		#dspbuff, W0	
	mov		#384, W1
	add		W0, W1, W1
	add		W1, W2, W1
	mov		#35, W13
	mov		W11, W9
b3:
	mov.b	[W9+1], W7
	mov.b	[W9+0], W6
	repeat	#5
	sl.b	W7, W7
	repeat	#1
	lsr.b	W6, W6
	add.b	W7, W6, W0
	mov.b	W0, [W1++]
	add		W9, #3, W9
	dec		W13, W13
	bra		NZ, b3

	mov		#dspbuff, W0	
	mov		#256, W1
	add		W0, W1, W1
	add		W1, W2, W1
	mov		#35, W13
	mov		W11, W9
	mov		W10, W8
b2:
	mov.b	[W9+0], W7
	mov.b	[W8+2], W6
	repeat	#5
	sl.b	W7, W7
	add.b	W7, W6, W0
	mov.b	W0, [W1++]
	add		W9, #3, W9
	add		W8, #3, W8
	dec		W13, W13
	bra		NZ, b2

	mov		#dspbuff, W0	
	mov		#128, W1
	add		W0, W1, W1
	add		W1, W2, W1
	mov		#35, W13
	mov		W10, W9
b1:
	mov.b	[W9+1], W0
	mov.b	W0, [W1++]
	add		W9, #3, W9
	dec		W13, W13
	bra		NZ, b1

	mov		#dspbuff, W0	
	mov		#0, W1
	add		W0, W1, W1
	add		W1, W2, W1
	mov		#35, W13
	mov		W10, W9
b0:
	mov.b	[W9+0], W0
	mov.b	W0, [W1++]
	add		W9, #3, W9
	dec		W13, W13
	bra		NZ, b0

	return


set_time_date:

	mov		#86, W3
	mov		#psvoffset(sd0), W0
	mov		#dspbuff, W2
	add		W2, W3, W2
	mov		#118, W3
	mov		tts1, W10
	mul.uu	W10, #10, W10
	add		W10, W0, W1
	repeat	#4
	mov		[W1++], [W2++]
	add		W2, W3, W2
	mov		tts10, W10
	mul.uu	W10, #10, W10
	add		W10, W0, W1
	repeat	#4
	mov		[W1++], [W2++]
	add		W2, W3, W2
	mov		#psvoffset(sdc), W1
	repeat	#4
	mov		[W1++], [W2++]
	add		W2, W3, W2
	mov		ttm1, W10
	mul.uu	W10, #10, W10
	add		W10, W0, W1
	repeat	#4
	mov		[W1++], [W2++]
	add		W2, W3, W2
	mov		ttm10, W10
	mul.uu	W10, #10, W10
	add		W10, W0, W1
	repeat	#4
	mov		[W1++], [W2++]
	add		W2, W3, W2
	mov		#psvoffset(sdc), W1
	repeat	#4
	mov		[W1++], [W2++]
	add		W2, W3, W2
	mov		tth1, W10
	mul.uu	W10, #10, W10
	add		W10, W0, W1
	repeat	#4
	mov		[W1++], [W2++]
	add		W2, W3, W2
	mov		tth10, W10
	mul.uu	W10, #10, W10
	add		W10, W0, W1
	repeat	#4
	mov		[W1++], [W2++]

	mov		#102, W3
	mov		#psvoffset(sd0), W0
	mov		#dspbuff, W2
	add		W2, W3, W2
	mov		#118, W3
	mov		s1, W10
	mul.uu	W10, #10, W10
	add		W10, W0, W1
	repeat	#4
	mov		[W1++], [W2++]
	add		W2, W3, W2
	mov		s10, W10
	mul.uu	W10, #10, W10
	add		W10, W0, W1
	repeat	#4
	mov		[W1++], [W2++]
	add		W2, W3, W2
	mov		#psvoffset(sdc), W1
	repeat	#4
	mov		[W1++], [W2++]
	add		W2, W3, W2
	mov		m1, W10
	mul.uu	W10, #10, W10
	add		W10, W0, W1
	repeat	#4
	mov		[W1++], [W2++]
	add		W2, W3, W2
	mov		m10, W10
	mul.uu	W10, #10, W10
	add		W10, W0, W1
	repeat	#4
	mov		[W1++], [W2++]
	add		W2, W3, W2
	mov		#psvoffset(sdc), W1
	repeat	#4
	mov		[W1++], [W2++]
	add		W2, W3, W2
	mov		h1, W10
	mul.uu	W10, #10, W10
	add		W10, W0, W1
	repeat	#4
	mov		[W1++], [W2++]
	add		W2, W3, W2
	mov		h10, W10
	mul.uu	W10, #10, W10
	add		W10, W0, W1
	repeat	#4
	mov		[W1++], [W2++]
	;
	mov		#118, W3
	mov		#dspbuff, W2
	add		W2, W3, W2
	mov		#118, W3
	mov		yy1, W10
	mul.uu	W10, #10, W10
	add		W10, W0, W1
	repeat	#4
	mov		[W1++], [W2++]
	add		W2, W3, W2
	mov		yy10, W10
	mul.uu	W10, #10, W10
	add		W10, W0, W1
	repeat	#4
	mov		[W1++], [W2++]
	add		W2, W3, W2
	mov		#psvoffset(sds), W1
	repeat	#4
	mov		[W1++], [W2++]
	add		W2, W3, W2
	mov		mm1, W10
	mul.uu	W10, #10, W10
	add		W10, W0, W1
	repeat	#4
	mov		[W1++], [W2++]
	add		W2, W3, W2
	mov		mm10, W10
	mul.uu	W10, #10, W10
	add		W10, W0, W1
	repeat	#4
	mov		[W1++], [W2++]
	add		W2, W3, W2
	mov		#psvoffset(sds), W1
	repeat	#4
	mov		[W1++], [W2++]
	add		W2, W3, W2
	mov		dd1, W10
	mul.uu	W10, #10, W10
	add		W10, W0, W1
	repeat	#4
	mov		[W1++], [W2++]
	add		W2, W3, W2
	mov		dd10, W10
	mul.uu	W10, #10, W10
	add		W10, W0, W1
	repeat	#4
	mov		[W1++], [W2++]

	return

send_command:
	bclr	LATA, #3
	mov.b	WREG, LATBH
	call	strobe
	return
send_data:
	bset	LATA, #3
	mov.b	WREG, LATBH
	call	strobe
	return

strobe:
	repeat	#10
	nop	
	bset	LATA, #4
	repeat	#10
	nop
	bclr	LATA, #4
	repeat	#10
	nop	
	return

delay1us:
	repeat	 #2000
	nop
	return

delay10us:
	repeat	 #10000
	nop
	repeat	 #10000
	nop
	return

init_PSV:
	mov		#psvpage(d0), W0
	mov		W0, PSVPAG		; set PSVPAG to page that contains hello
	bset.b	CORCONL,#PSV	; enable Program Space Visibility
	return

wait1s:
	push	W0
	mov		#200, W0
wait1sloop:
	repeat	#10000
	nop
	dec		W0, W0
	bra		NZ, wait1sloop	
	pop		W0
	return

separate_nibbles:
	push	W1
	mov		#0x000F, W1
	mov		W0, W10
	mov		W0, W11
	and		W10, W1, W10
	lsr		W11, #4, W11
	and		W11, W1, W11
	pop		W1
	return
.end                               ;End of program code in this file