' GPS Speedometer Version 4
' 17 July 2015 - installed on final GPS Speedo
'
' reads data from Adafruit ultimate gps module
' displays on Adafruit backpack LED module
' includes a maximum speed option by grounding Pin 23

'  		constants and codes for speeds 0-9

dim num(10) = 0x3f, 0x06, 0x5b, 0x4f, 0x66, 0x6d, 0x7d, 0x07, 0x7f, 0x6f
ht15k33 = 0xe0		' addr of adafruit backpack card left-shifted 1 bit
osc_on  = 0x21		' code to turn oscillator on
disp_on = 0x81		' code to turn display on, steady
disp_off = 0x80		' code to turn display off
blink = 0x83		' code to blink display
bright  = 0xef		' code for max brightness
z$ = chr$(00)		' code for a blank digit/colon
CR$ = chr$(13)		' carriage return
LF$ = chr$(10)		' line feed
pinmode 23,in		' Pin 23 set to INPUT
max_speed = 0		' clear max speed variable

' 		initialize the I2C bus on the EZsbc1 card

i2cinit(100000)		' initialize the I2C bus
i2ctime(10)

' 		INIT LED BACKPACK CARD

c$=""
i2cwr(0xe0,osc_on,c$,0)	' trick to send <adr> <cmd> only
delay(100)
i2cwr(0xe0,bright,c$,0)	' max brightness
delay(100)
i2cwr(0xe0,blink,c$,0)	' display on, BLINKING until get a fix
delay(100)
speed = 8888
gosub display4dig:	' set display to all eights

' 		INIT SERIAL PORT TO 38400 baud

serinit 1,9600,8,1,0	' Port 1, 9600 baud N-8-1

'		INIT GPS CHIP

'	Set updates at 5/sec
'	output only RMC and VTG sentences
'	send RMC every 5, VTG every 3 updates

b2$ = "$PMTK251,38400*27"+CR$+LF$
s2$ = "$PMTK220,200*2C"+CR$+LF$
u2$ = "$PMTK314,0,5,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*2C"+CR$+LF$

x = serout(1,b2$)	' change baud rate on gps to 38400
? b2$
if x<0 then 
	? "Error sending ",b2$
endif
serinit 1,38400,8,1,0	' change EXsbc1 baud rate to 38400
delay(500)

x = serout(1,s2$)
? s2$
if x<0 then ? "Error sending ",s2$
endif
delay(100)

x = serout(1,u2$)
? u2$
if x<0 then ? "Error sending ",u2$
endif
delay(100)

rmc$ = "$GPRMC"		' rec'd minimum output
vtg$ = "$GPVTG"		' speed over ground output
fix$ = ",A,"		' signifies lockon in RMC sentence
xs=0

'		INITIAL LOOP; WAIT UNTIL GET A FIX
speed = 0
repeat
	s1$ = serinp$(1,99,10,75000)	' read a line ending w/LF
	IF LEFT$(s1$,6)=rmc$ then
	speed = speed+1
		gosub display4dig:	' display countup #
		print speed;
		print s1$
		xs = instr(s1$,fix$,1)	' find "A" in message
	endif
until xs>0 or inkey>-1			' easy way to exit1
print "Lock-on found!"
i2cwr(0xe0,disp_on,c$,0)		' display on, BLINKING off
delay(100)

'		MAIN LOOP TO READ GPS, DISPLAY SPEED

repeat
	s$ = serinp$(1,99,10,50000)	' read a line ending w/LF
	if left$(s$,6) = vtg$ then
		gosub parsevtg:		' extract speed in mph
		gosub display4dig:	' display on LED
		delay(200)
	endif
until inkey > -1			' exit with keypress on USB host

end					' end of program

' ------------------------------------------------------------------------
'	             SUBROUTINE TO PARSE VTG SENTENCE
' ------------------------------------------------------------------------
parsevtg:
comma$ = ","
' read and parse VTG sentence
mag$ = ",M,"		' precedes speed in knots
l2 = len(s$)		' length of test string

'  Search for location of mag$ string in s$
xs = instr(s$,mag$,10)

if xs>0 then		' find speed value following mag$
	xl = instr(mid$(s$,xs+3),comma$,xs+1)
	v$ = mid$(s$,xs+3,xs+3+xl)
	xl = instr(v$,comma$,xl)
	v$ = mid$(v$,1,xl-1)
	speed = val(v$) * 1.151		' convert knots to mph
	speed = int(10*speed+0.5)	' round to nearest mph*10
	if speed > max_speed then
		max_speed = speed	' save max speed
	endif
	if ind(23)=0 then
		delay(500)		' simple de-bounce
		if ind(23)=0 then
			speed = max_speed
		endif
	endif
endif

return

'-------------------------------------------------------------------------
'	              SUBROUTINE TO DISPLAY SPEED
'-------------------------------------------------------------------------

' subroutine to display up to 4 digits with leading zero blanking
' call with an integer in the variable 'number'
' format is xxx.x; if digit 3 is zero, display 0.x

display4dig:
	number = speed;
	d1=int(number/1000)
	d2=int((number-d1*1000)/100)
	d3=int((number-d1*1000-d2*100)/10)
	d4=int((number-d1*1000-d2*100-d3*10))
	if d1=0 then
		d1$=chr$(0x00)
		if d2=0 then
			d2$=chr$(0x00)
		else
			d2$=chr$(num(d2+1))
		endif
		if d3=0 then 
			d3$=chr$(0xbf)
		else
			d3$=chr$(num(d3+1)+128)
		endif
	else
		d1$=chr$(num(d1+1))
		d2$=chr$(num(d2+1))
		d3$=chr$(num(d3+1)+128)
	endif
	d4$=chr$(num(d4+1))		' always display trailing 0
	
	z1$=chr$(16)
	i2cwr(0xe0,0x00,d1$,1)
	i2cwr(0xe0,0x02,d2$,1)
	i2cwr(0xe0,0x04,z1$,1)	' clear colons
	i2cwr(0xe0,0x06,d3$,1)
	I2CWR(0Xe0,0x08,d4$,1)

	i2cwr(0xe0,0x81,z$,0)	' blink off
	
return