Function Commapos(startcomma As Byte , Header As String) As String
Local N As Byte , Q As Byte , Comma As Byte , X As String * 1 , Endcomma As Byte , Ch As String * 1 , Ggps As String * 5
Local Gps1 As String * 80
'CLEAR BUFFER, LOOPS 'TILL NOTHING LEFT
  While Ischarwaiting() > 0
   Ch = Inkey()
   Wend

'SCROLL BUFFER AND GET GET THE SENTENCE THAT STARTS WITH "HEADER"  IE - GPRMC,GPGGL......
    Ggps = ""                                               'CLEAR GGPS JUST IN CASE
    While Ggps <> Header
       Do
       Ch = Inkey()
       Loop Until Ch = "$"
    Input , Gps1 Noecho                                     'GET A FULL SENTENCE
    Ggps = Left(gps1 , 5)                                   'CHECK IT FOR A MATCH
    Wend
 'n=start posistion  q=end posistion
 'q also = grab string length ie- mid(var,n,q)
 'find index(N) for start comma posistion
  Comma = 0 : N = 0 : Q = 0                                 'INITIALISE VARIABLES
  Endcomma = Startcomma + 1
    While Comma < Startcomma                                'COMMA HOLD THE COMMA COUNT AMOUNT
     Incr N
     X = Mid(gps1 , N , 1)
     If X = "," Then Incr Comma
     Wend
  'find index(Q) for end comma posistion
    Q = N
    While Comma < Endcomma
     Incr Q
     X = Mid(gps1 , Q , 1)
     If X = "," Then Incr Comma
    Wend

 'not quite right-TWO COMMAS IN A ROW CAUSE PROBLEMS  - fix it here
 N = N + 1
 If Q = N Then
 Cls                                                        'START COUNT AND END COUNT NEED A SEPERATION OF MORE THAN 0
   Lcd "GOT A DOUBLE"                            'just for debugging - maybe a good way to check for a VALID sentence
   Wait 1
 End If
  N = N - 1
  Q = Q - N
  Q = Q - 1
  N = N + 1
  'suck out the needed string componant
  Commapos = Mid(gps1 , N , Q)
  End Function

Function Convert(field1 As String , Field2 As String , ) As Single
Local Y As String * 7 , X As String * 3 , Z As Byte , W As Single

Z = 0 : X = "" : Y = ""

Z = Len(field1)
Y = Right(field1 , 7)

If Z = 9 Then X = Left(field1 , 2)
If Z = 10 Then X = Left(field1 , 3)

 W = Val(x)
 Convert = Val(y)
 Convert = Convert / 60
 Convert = Convert + W
'make it neg for south or west
 If Field2 = "S" Then Convert = -convert
 If Field2 = "W" Then Convert = -convert
 Convert = Convert
 End Function
 

'======[ Calc Distance of lat lon ]=============================================
Sub CalcBearing(CB_Lat1, CB_Lon1, CB_Lat2, CB_Lon2)

'using arcsin formula rather than arccos
'calc dist and bearing in radians of pos2 from pos1
'positions in radians
'result in nm and true bearing from north
'x1,y1 are long,lat of source, x2, y2 long, lat of dest


    Local CB_X1    as single   : Local CB_Y1 As single
    Local CB_X2    as single   : Local CB_Y2 As single

    Local CB_Dx    as single   : Local CB_Dy As single

    Local CB_Cosd  as single
    Local CB_Nu    as single
    Local CB_De    as single
    Local CB_Be    as single
    Local CB_Ad    as single
    Local CB_Dq    as single

    CB_Dx= CB_Lon2 - CB_Lon1   : CB_Dx= CB_Dx / 2
    CB_Dx= Sin(CB_dx)          : CB_Dx= CB_Dx * CB_Dx
    CB_Dy= CB_Lat2 - CB_Lat1   : CB_Dy= CB_Dy / 2
    CB_Dy= Sin(CB_dy)          : CB_Dy= CB_Dy * CB_Dy

'the arc cos function fails for very small dx
'cosd = Sin(Y1) * Sin(Y2) + Cos(Y1) * Cos(Y2) * Cos(X2 - X1)
'so use:
'd=2*asin(sqrt((sin((lat1-lat2)/2))^2 + cos(lat1) * cos(lat2) *(sin((lon1 -lon2) / 2)) ^ 2))
'be=atan((sin(lon1-lon2)*cos(lat2))/(cos(lat1)*sin(lat2)-sin(lat1)*cos(lat2)*cos(lon1-lon2))

    CB_X1= sin(CB_lat1)  : CB_Y1= Cos(CB_lat1)
    CB_X2= sin(CB_lat2)  : CB_Y2= Cos(CB_lat2)
    CB_Dx= CB_Dx * CB_Y1 : CB_Dx= CB_Dx * CB_Y2
    CB_De= CB_Dy + CB_Dx : CB_De= Sqr(CB_de)

    CB_Dq= Asin(CB_de)   : CB_Dq = CB_Dq * 2.0   ' Dq gives distance in radians

    GPSDist = CB_Dq * CB_Crad2meters

    'now calc bearing
    CB_Dx= CB_Lon2 - CB_Lon1 : CB_Nu= sin(CB_Dx) : CB_Nu= CB_nu * CB_Y2


    CB_De= cos(CB_Dx) :
    CB_De= CB_De * CB_Y2 : CB_De= CB_De * CB_X1
    CB_X2= CB_X2 * CB_Y1 : CB_De= CB_X2 - CB_De

    CB_Ad = 0.0

    If CB_Nu <> 0.0 Then
       If CB_De <> 0.0 Then
          CB_Be= CB_Nu / CB_De
          CB_Be= Atn(CB_be)                     ' bearing in radians
          If CB_De < 0.0 Then
             CB_Ad = CB_Cpi
          EndIf
          If CB_Nu < 0.0 And CB_De > 0.0 Then
             CB_Ad= CB_C2pi
          EndIf
       Else
          If CB_Lon2 > CB_Lon1 Then
             CB_Ad = CB_Cpi / 2.0
          Else
             CB_Ad = 1.5 * CB_Cpi
          End If
       End If
    Else
       If CB_Lat2 > CB_Lat1 Then
          CB_Be = 0.0
       Else
          CB_Be = CB_Cpi
       End If
    End If
    CB_Be= CB_Be + CB_Ad

    GPSBear = RAD2DEG(CB_be)
    if GPSBear <> 0.0 then GPSBear = 360.0 - GPSBear

End Sub
 

 '####################################################################################################################
 '3d distance calculation function
 'altitude =  in meters
 'lat lon must be in decimal
 'output in kilometers

Function 3ddistance(startlat , Startlon , Endlat , Endlon , Startalt , Endalt)as Single
Local R As Single , D As Single , X As Single , Y As Single
Local Tmp1 As Single , Tmp2 As Single , Tmp3 As Single , Height As Single

'first convert decimal lat lon to radians
'r=d*pi/180

Startlat = Startlat * 3.14158 : Startlat = Startlat / 180
Startlon = Startlon * 3.14158 : Startlon = Startlon / 180
Endlat = Endlat * 3.14158 : Endlat = Endlat / 180
Endlon = Endlon * 3.14158 : Endlon = Endlon / 180

'now lets butcher some maths
'2d distance - simple method ok for small distance
'ripped from this site - http://www.movable-type.co.uk/scripts/latlong.html
'x=(startlon-endlon)*cos((startlat+endlat)/2)
'y=(endlat-startlat)
'distance=sqrt(x*x+y*y)*6371     6371=earths radius in k's

Tmp1 = 0 : Tmp2 = 0 : Tmp3 = 0

 Tmp1 = Startlon - Endlon
 Tmp2 = Startlat + Endlat
 Tmp2 = Tmp2 / 2
 Tmp3 = Cos(tmp2)
 X = Tmp1 * Tmp3

 Y = Endlat - Startlat

 Tmp1 = X * X
 Tmp2 = Y * Y
 Tmp3 = Tmp1 + Tmp2
 Tmp3 = Sqr(tmp3)

 'd=2d distance in kilometers
 D = Tmp3 * 6371

'now the 3d distance
'3d*3d=(d*d)+(height*height)

 Height = Endalt - Startalt
 'convert from meters to kilometers
 Height = Height / 1000

 Tmp1 = D * D
 Height = Height * Height

 Tmp2 = Tmp1 + Height

 3ddistance = Sqr(tmp2)
 End Function

 

'----------------------------------------------------------------------------
'                  (c) 1995-2008
'                 MCS Electronics
' This program will calculate the distance between GPS coordinates
' It also calculates course
'----------------------------------------------------------------------------
$regfile = "m128def.dat"
$framesize = 320
$hwstack = 160
$swstack = 160
$crystal = 16000000
'-------------------------------------------------------------------------------
Const Pi = 3.14159265358979
Const Mpi = Pi / 180
Const Radiusearth = 6371000

Dim Calc_distance As Double
Dim Calc_course_d As Double
Dim Calc_course_m As Double
Dim Calc_course_s As Double

Declare Sub Get_distance_course(byval Lat1 As Double , Byval Lon1 As Double , Byval Lat2 As Double , Byval Lon2 As Double)
'-------------------------------------------------------------------------------

Call Get_distance_course( -9.9666 , 18.0374 , 52.8273 , -8.4409)

If Calc_distance > 1000 Then Calc_distance = Calc_distance / 1000

Print "Distance: " ; Calc_distance
Print "Course: " ; Calc_course_d ; "D " ; Calc_course_m ; "M " ; Calc_course_s ; "S"

End
'-------------------------------------------------------------------------------
Sub Get_distance_course(byval Lat1 As Double , Byval Lon1 As Double , Byval Lat2 As Double , Byval Lon2 As Double)

  Local Tmp As Double , A As Double , B As Double , C As Double , T2 As Double

        ' 1 Degree is 69.096 miles, 1 mile is 1609.34 m
        A = Lat1 * Mpi                                      'Mpi
        A = Cos(a)
        T2 = Lat2 * Mpi : T2 = Cos(t2) : A = A * T2
        T2 = Lon1 * Mpi : T2 = Cos(t2) : A = A * T2
        T2 = Lon2 * Mpi : T2 = Cos(t2) : A = A * T2

        B = Lat1 * Mpi : B = Cos(b)
        T2 = Lon1 * Mpi : T2 = Sin(t2) : B = B * T2
        T2 = Lat2 * Mpi : T2 = Cos(t2) : B = B * T2
        T2 = Lon2 * Mpi : T2 = Sin(t2) : B = B * T2

        C = Lat1 * Mpi : C = Sin(c)
        T2 = Lat2 * Mpi : T2 = Sin(t2) : C = C * T2

        T2 = A + B
        T2 = T2 + C
        A = Abs(t2)

        If A >= 1 Then
           Calc_distance = 0
        Else
           T2 = Acos(t2) : Tmp = T2 : T2 = T2 * Radiusearth
           Calc_distance = T2
        End If


       'Calculate bearing course
        A = Lat2 * Mpi : A = Sin(a)

        B = Lat1 * Mpi : B = Sin(b)
        T2 = Cos(tmp) : B = B * T2

        C = Lat1 * Mpi : C = Cos(c)
        T2 = Sin(tmp) : C = C * T2

        A = A - B
        A = A / C
        A = Acos(a)
        A = Rad2deg(a)

        B = Lon2 * Mpi
        C = Lon1 * Mpi
        Tmp = B - C

        If Tmp < 0.0 Then A = 360 - A

        Calc_course_d = Int(a)
        Tmp = Frac(a)
        Tmp = Tmp * 60
        Calc_course_m = Int(tmp)
        Tmp = Frac(tmp)
        Tmp = Tmp * 60
        Calc_course_s = Round(tmp)
End Sub

'anchor / proximity alarm returns "anchor" in meters from target when setdistance is exceded
'lat,lon in decimal                                       meters,
'to_from.. 0=to 1=from ie 1=anchor and 0=proximity alarm conviguration
Function Anchor(anchlat , Anchlon , Curlat , Curlon , Setdistance , To_from) As Single
Local Tmp As Single
Setdistance = Setdistance / 1000
   'couldnt be stuffed rewiting the dist function here. if stack / portability is an issue rewrite it
   Tmp = 3ddistance(anchlat , Anchlon , Curlat , Curlon , 1 , 1)

   If To_from = 1 And Tmp > Setdistance Then
     Tmp = -tmp
     Elseif To_from = 0 And Tmp < Setdistance Then
     Tmp = -tmp

      End If

'convert to meters
Tmp = Tmp * 1000
Anchor = Tmp

End Function

End Function
 

Function Bearing(lat1 , Lon1 , Lat2 , Lon2)as Single

'using arcsin formula rather than arccos
'calc dist and bearing in radians of pos2 from pos1
'positions in de degrees converted here to radians
'result = true bearing from north
'x1,y1 are long,lat of source, x2, y2 long, lat of dest

Const Cpi = 3.14159265
Const C2pi = 6.2831854
Const Crad2nm = 3437.746771                                 ' radians to nautical miles
Const Cnm2ft = 6080.0                                       ' nautical miles to feet



Local Bear As Single
Local X1 As Single
Local X2 As Single
Local Y1 As Single
Local Y2 As Single
Local Dx As Single

Local Dy As Single
Local Cosd As Single
Local Nu As Single
Local De As Single
Local Be As Single
Local Ad As Single
Local Dq As Single
'conver decimal lat,lons to radians
Lat1 = Lat1 * Cpi : Lat1 = Lat1 / 180
Lat2 = Lat2 * Cpi : Lat2 = Lat2 / 180
Lon1 = Lon1 * Cpi : Lon1 = Lon1 / 180
Lon2 = Lon2 * Cpi : Lon2 = Lon2 / 180


 Dx = Lon2 - Lon1 : Dx = Dx / 2 : Dx = Sin(dx) : Dx = Dx * Dx
 Dy = Lat2 - Lat1 : Dy = Dy / 2 : Dy = Sin(dy) : Dy = Dy * Dy
'the arc cos function fails for very small dx
'cosd = Sin(Y1) * Sin(Y2) + Cos(Y1) * Cos(Y2) * Cos(X2 - X1)
'so use:
'd=2*asin(sqrt((sin((lat1-lat2)/2))^2 + cos(lat1) * cos(lat2) *(sin((lon1 -lon2) / 2)) ^ 2))
'be=atan((sin(lon1-lon2)*cos(lat2))/(cos(lat1)*sin(lat2)-sin(lat1)*cos(lat2)*cos(lon1-lon2))

 X1 = Sin(lat1) : Y1 = Cos(lat1)
 X2 = Sin(lat2) : Y2 = Cos(lat2)
 Dx = Dx * Y1 : Dx = Dx * Y2

 De = Dy + Dx : De = Sqr(de)
 Dq = Asin(de) : Dq = Dq * 2.0

 'now calc bearing
 Dx = Lon2 - Lon1
 Nu = sin(Dx)
 Nu = nu * Y2

 De = Cos(dx) : De = De * Y2 : De = De * X1 : X2 = X2 * Y1 : De = X2 - De
 Ad = 0.0

If Nu <> 0.0 Then
   If De <> 0.0 Then
     Be = Nu / De
     Be = Atn(be)                                            ' bearing in radians
     If De < 0.0 Then
       Ad = Cpi
     End If
     If Nu < 0.0 And De > 0.0 Then
       Ad = C2pi
     End If

   Else
     If Lon2 > Lon1 Then
       Ad = Cpi / 2.0
     Else
       Ad = 1.5 * Cpi

     End If
   End If
Else
   If Lat2 > Lat1 Then
     Be = 0.0
   Else
     Be = Cpi
   End If
End If
 Be = Be + Ad

 Bear = Be                                                  ' in radians
'to convert from radians to degrees, multiply by 180/pi.
 Tmp1 = 180 / Cpi
 Bear = Bear * Tmp1
 Bearing = Bear
End Function

 Function Speed(knots As String)as String
  Local Oput As String * 10 : Local X As Single : Local Y As Byte : Local Z As Byte

  X = Val(knots)
  X = X * 1.852
  Oput = Str(x)
  Y = Charpos(oput , ".")

 Z = Y + 3 : Insertchar Oput , Z , " "
 Z = Y + 4 : Insertchar Oput , Z , "K"
 Z = Y + 5 : Insertchar Oput , Z , "p"
 Z = Y + 6 : Insertchar Oput , Z , "h"

 Oput = Left(oput , Z)
 Speed = Oput
 End Function