For those of you interested in GPS stuff... Attached code for a GPS interpreter that reads and parses NMEA messages coming in to USART, and writes them to LCD. In addition, it will allow you to store your present location to EEPROM and from then on calculate instructions on how to get back there.
This version was compiled on MB 4.02. It runs on a P18F452.
Marcel
Code: Select all
'GPS interpreter
'Marcel Durieux, 2006
'Reads in NMEA strings from any GPS receiver unit (over USART), and outputs (on LCD)
'location, speed, course, altitude, accuracy and number of satellites in view, as well as
'universal date and time.
'In addition, it allows saving a current location (in NV memory) and will calculate distance,
'direction and deviation from the desired direction. These calculations are performed
'using great-circle formulae.
'Not implemented, but easy to add, is serial output of the results to some other device.
'The program interprets two of the many NMEA strings, RMC and GGA. Since there
'are slight differences in NMEA versions (# of digits, etc), here are examples
'of valid strings for this program.
'$GPRMC,214200.875,A,3806.5677,N,07826.1267,W,000.0,000.0,120206,009.7,W*69
'$GPGGA,214201.875,3806.5677,N,07826.1267,W,1,05,01.8,00124.4,M,-34.3,M,,*51
'A good overview of the various NMEA output is available at http://aprs.gids.nl/nmea/
'The program runs on a P18F452 clocked at 20 MHz.
program GPS
'data variables
dim Gstr as string[65] 'to hold NMEA sentence, subsequently to be dissected into:
dim LaD as string[2] 'Latitude degrees
dim LaM1 as string[2] 'Latitude whole minutes
dim LaM2 as string[4] 'Latitude fractional minutes
dim LoD as string[3] 'Longitude degrees
dim LoM1 as string[2] 'Longitude whole minutes
dim LoM2 as string[4] 'Longitude fractional minutes
dim LoDir as char 'Longitude direction (E or W)
dim LaDir as char 'Latitude direction (N or S)
dim Kspeed as string[3] 'Speed in knots (as read from device)
dim Crs as string[3] 'Course
dim Galt as string[4] 'Altitude
dim Gacc as string[3] 'Accuracy
dim Gstatus as char 'Status: valid (A) or invalid (V) fix
dim Gsats as string[2] 'number of satellites in view
dim GDate as string[6] 'date
dim GTime as string[6] 'time
'calculated values
dim Mspeed as string[3] 'Calculated speed in mph
dim Cdir as integer ' store numeric version of Crs
dim Dev as integer 'deviation from desired course
dim La1,La2,Lo1,Lo2 as float 'numeric values of Latitude, Longitude
dim Direct,Dist as float 'calculated direction and distance to target
'utility variables
dim i as byte
dim tmp as float
dim GMode as byte 'to hold display mode. The program cycles through 4 display
'modes by briefly pulling either B.7 or B.6 low:
'1. coordinates and number of satellites in view
'2. speed, course, altitude and accuracy
'3. date and time
'4. distance, direction and deviation to target location
dim fnc as byte 'to cycle through NMEA sentences: 1=RMC, 2=GGA
dim WriteFlag as byte 'determines if display needs updating
dim FltStr as string[17] 'to display floats on LCD
dim IntStr as string[6] 'to display integers on LCD
'subroutines:
sub function sq(dim a as real) as real 'calculates square
Result=a*a
end sub
sub function ReadChr as byte 'reads one char from USART port
do
loop until USART_Data_Ready = 1
result=USART_Read 'read char
end sub
sub procedure CopyGStr(dim start as byte, dim l as byte, dim byref txt as string[10])
'extracts 'l' chars from 'Gstr' (which holds the NMEA sentence) starting at 'start'
dim i as byte
for i=0 to l
txt[i]=Gstr[start+i]
next i
txt[l]=chr(0) 'to terminate the string
end sub
main:
'device setup
TRISB=$FF 'two buttons will be connected to B.7 and B.6. Pulling B.6 low
'cycles through the 4 modes (see above), pulling both low saves current location
PORTB=0
TRISD=0 'for 2x16 CLD char unit
USART_Init(4800) 'init USART at 4800 baud to interface with NMEA device
LCD_Init(PORTD) 'init LCD on port D
LCD_Cmd(LCD_Cursor_Off)
LCD_Cmd(LCD_Clear)
'initialize variables
Gstatus="V" 'assume invalid fix at startup
fnc = 1 'get ready to look for first NMEA string
WriteFlag=0 'no data to display yet
GMode=1 'location display
Gsats="00" 'no satellites in view
while true 'main program loop
'monitor NMEA data until a '$' is received, signaling start of a sentence
do
i=ReadChr
loop until i=36 'loop until "$" received
'read sentence (from '$' through '*') into the variable Gstr
i=0
do
Gstr[i]=ReadChr
if Gstr[i]="*" then
i=0
else
i=i+1
end if
loop until (i=0) or (i=65) 'break at 65 chars if for some reason no '*' is found
'now look for RMC or GGA, depending on 'fnc' value.
if fnc=1 then 'look for RMC
if ((Gstr[2]="R") and (Gstr[3]="M") and (Gstr[4]="C")) then
fnc=2 'next read will read GGA
WriteFlag=1 'get ready to display data
i=Gstatus
Gstatus=Gstr[17] 'get status char
if Gstatus<>i then 'i.e. if status has changed
LCD_Cmd(LCD_Clear)
end if
if Gstatus="A" then ' valid fix, copy the variable from Gstr
CopyGStr(6,6,GTime)
CopyGStr(56,6,GDate)
CopyGStr(19,2,LaD) 'get latitude
CopyGStr(21,2,LaM1)
CopyGStr(24,4,LaM2)
LaDir=GStr[29] 'get latitude direction
CopyGStr(31,3,LoD) 'get longitude
CopyGstr(34,2,LoM1)
CopyGstr(37,4,LoM2)
LoDir=GStr[42] 'get longitude direction
CopyGStr(44,3,Kspeed) 'get speed and convert to mph
i=(Kspeed[0]*100+Kspeed[1]*10+Kspeed[2])*1.15 'first numeric
ByteToStr(i,Mspeed) 'then back to string for display
CopyGStr(50,3,Crs) 'get course
Cdir=(Crs[0]-48)*100+(Crs[1]-48)*10+Crs[2]-48 'convert to number
end if
end if
end if
if fnc=2 then 'look for GGA
if ((Gstr[2]="G") and (Gstr[3]="G") and (Gstr[4]="A")) then
fnc=1 'next time round, look for RMC
WriteFlag=1 'update display
CopyGStr(44,2,Gsats) 'get #sats
if Gstatus="A" then
CopyGStr(48,3,Gacc) 'get accuracy
CopyGstr(53,4,Galt) 'get altitude
end if
end if
end if
'if we have valida data (i.e. WriteFlag=1) then...
if WriteFlag=1 then
'1. check if the user wants this location stored in EEPROM
if (PORTB.6=0) and (PORTB.7=0) then 'if BOTH buttons pressed
LCD_Cmd(LCD_Clear)
LCD_Out(1,1,"Location saved") 'store current location in EEPROM
'subtracting 48 is to get from ASCII to numeric value
EEPROM_Write(0,LaD[0]-48)
EEPROM_Write(1,LaD[1]-48)
EEPROM_Write(2,LaM1[0]-48)
EEPROM_Write(3,LaM1[1]-48)
EEPROM_Write(4,LaM2[0]-48)
EEPROM_Write(5,LaM2[1]-48)
EEPROM_Write(6,LaM2[2]-48)
EEPROM_Write(7,LaM2[3]-48)
EEPROM_Write(8,LoD[0]-48)
EEPROM_Write(9,LoD[1]-48)
EEPROM_Write(10,LoD[2]-48)
EEPROM_Write(11,LoM1[0]-48)
EEPROM_Write(12,LoM1[1]-48)
EEPROM_Write(13,LoM2[0]-48)
EEPROM_Write(14,LoM2[1]-48)
EEPROM_Write(15,LoM2[2]-48)
EEPROM_Write(16,LoM2[3]-48)
EEPROM_Write(17,LaDir)
EEPROM_Write(18,LoDir)
delay_ms(5000)
LCD_Cmd(LCD_Clear)
end if
'2. check if the user wants to change modes
if PORTB.6=0 then ' if B.6 is pulled low
GMode=Gmode+1 'cycle through modes
if Gmode=5 then
Gmode=1
end if
LCD_Cmd(LCD_Clear)
do
loop until PORTB.7=1 'button released
end if
'and here are the disaply routines. First, if no valid data is available...
if Gstatus="V" then 'no valid fix
LCD_Out(1,1,"Searching...")
else 'valid fix
select case GMode
case 1 'location display
LCD_Chr(1,1," ")
LCD_Out(1,2,LaD)
LCD_Chr(1,4,"*")
LCD_Out(1,5,LaM1)
LCD_Chr(1,7,".")
LCD_Out(1,8,LaM2)
LCD_Chr(1,12,LaDir)
LCD_Chr(1,14,"S")
LCD_Out(1,15,Gsats)
LCD_Out(2,1,LoD)
LCD_chr(2,4,"*")
LCD_Out(2,5,LoM1)
LCD_Chr(2,7,".")
LCD_Out(2,8,LoM2)
LCD_Chr(2,12,LoDir)
case 2 ' speed, direction, altitude, accuracy
LCD_Out(1,1,"Spd:")
LCD_Out(1,5,KSpeed)
LCD_Out(1,10,"Crs:")
LCD_Out(1,14,Crs)
LCD_Out(2,1,"Alt:")
LCD_Out(2,5,Galt)
LCD_Out(2,10,"Acc:")
LCD_Out(2,14,Gacc)
case 3 'time, date
LCD_Out(1,1,"Date:")
LCD_Chr(1,7,GDate[2])
LCD_Chr(1,8,GDate[3])
LCD_Chr(1,9,"-")
LCD_Chr(1,10,GDate[0])
LCD_Chr(1,11,GDate[1])
LCD_Chr(1,12,"-")
LCD_Chr(1,13,GDate[4])
LCD_Chr(1,14,GDate[5])
LCD_Out(2,1,"Time:")
LCD_Chr(2,7,GTime[0])
LCD_Chr(2,8,GTime[1])
LCD_Chr(2,9,":")
LCD_Chr(2,10,GTime[2])
LCD_Chr(2,11,GTime[3])
LCD_Chr(2,12,":")
LCD_Chr(2,13,GTime[4])
LCD_Chr(2,14,GTime[5])
case 4 'directions to saved location. This is more complicated
'read target location from EEPROM and convert to numeric
La2=EEPROM_Read(0)*10+EEPROM_Read(1) 'degrees
tmp=(EEPROM_Read(2)*10+EEPROM_Read(3))/60 'minutes
La2=La2+tmp
tmp=(EEPROM_Read(4)*1000+EEPROM_Read(5)*100+EEPROM_Read(6)*10+EEPROM_Read(7))/600000 'dec minutes
La2=La2+tmp
Lo2=EEPROM_Read(8)*100+EEPROM_Read(9)*10+EEPROM_Read(10) 'degrees
tmp=(EEPROM_Read(11)*10+EEPROM_Read(12))/60 'minutes
Lo2=Lo2+tmp
tmp=(EEPROM_Read(13)*1000+EEPROM_Read(14)*100+EEPROM_Read(15)*10+EEPROM_Read(16))/600000 'dec minutes
Lo2=Lo2+tmp
if EEPROM_Read(17)<>$4e then '"N"
La2=-La2
end if
if EEPROM_Read(18)<>$57 then '"W"
Lo2=-Lo2
end if
'calculate numeric value of current position
La1=(LaD[0]-48)*10+LaD[1]-48
tmp=((LaM1[0]-48)*10+LaM1[1]-48)/60
La1=La1+tmp
tmp=((LaM2[0]-48)*1000+(LaM2[1]-48)*100+(LaM2[2]-48)*10+(LaM2[3]-48))/600000
La1=La1+tmp
if LaDir="S" then
La1=-La1
end if
Lo1=(LoD[0]-48)*100+(LoD[1]-48)*10+LoD[2]-48
tmp=((LoM1[0]-48)*10+(LoM1[1]-48))/60
Lo1=Lo1+tmp
tmp=((LoM2[0]-48)*1000+(LoM2[1]-48)*100+(LoM2[2]-48)*10+LoM2[3]-48)/600000
Lo1=Lo1+tmp
if LoDir="E" then
Lo1=-Lo1
end if
'here comes the trigonometry! Calculate direction and distance from
'current location to target.
'First, convert to radians
La1=La1*pi/180
Lo1=Lo1*pi/180
La2=La2*pi/180
Lo2=Lo2*pi/180
'calculate distance using great circle formula
Dist=2*Asin(sqrt(sq(sin((La1-La2)/2))+cos(La1)*cos(La2)*sq(Sin((Lo1-Lo2)/2))))
'calculate direction
Direct=acos((sin(La2)-sin(La1)*cos(Dist))/(sin(Dist)*cos(La1)))
if sin(Lo2-Lo1)>0 then
Direct=2*pi-Direct
end if
'convert results
Direct=(180/pi)*Direct 'convert to degrees
Dist=3982*Dist ' convert to miles
'calculate deviation
'0=moving straight towards target. Neg values=moving too far to the
'left. Pos values=moving too far to the right
Dev=CDir-Direct
if Dev>180 then
Dev=Dev-360
end if
if Dev<(-180) then
Dev=Dev+360
end if
'display
FloatToStr(Dist,FltStr)
LCD_Out(1,1,"Dist:")
LCD_out(1,7,FltStr)
IntToStr(Floor(Direct+0.5),IntStr)
LCD_Out(2,1,"Dir:")
LCD_Chr(2,5,IntStr[3])
LCD_Chr(2,6,IntStr[4])
LCD_Chr(2,7,IntStr[5])
LCD_Out(2,9,"Dev:")
IntToStr(Dev,IntStr)
LCD_Chr(2,13,IntStr[2])
LCD_Chr(2,14,IntStr[3])
LCD_Chr(2,15,IntStr[4])
LCD_Chr(2,16,IntStr[5])
end select
end if
WriteFlag=0
end if
wend
end.
