How to calculate Distance and Bearing between two NMEA GPS coordinates

[trastikata]

Calculating the distance and the bearing between two GPS coordinates is trivial in terms of information on How-To, however implementation in 32b math is not so trivial, especially on short distances where trigonometric results approach zero.

The code posted here is simplified version and the results have very good accuracy with 32b math, specifically for short distances, compared to 64b math. However, for long distances the accuracy is slightly reduced, given that the 64b math allows for use of the full (different) formulas.

Information was taken from here and here.

Note: In my applications I transmit or store the NMEA coordinates as signed 32b integers instead of strings, thus saving space. For example Latitude 2852.7780,N becomes 28527780, or Latitude 2852.7780,S will be -28527780, same for Longitude.

Code Select Expand

Device = 18F26J50

Declare Xtal = 20

Declare Reminders = OFF
Declare FSR_CONTEXT_SAVE = On
Declare LABEL_BANK_RESETS = On
Declare Optimiser_Level = 3
Declare Dead_Code_Remove = On
Declare Float_Display_Type = Large
Declare All_Digital = True

Dim Lat As Float            'Destination Point Latitude
Dim Lon As Float            'Destination Point Longitude
Dim LatOrg As Float         'Point of origin Latitude
Dim LonOrg As Float         'Point of origin Longitude

Dim Distance As Dword       'Distance between origin and destination
Dim Bearing As Word         'Bearing between origin and destination
Dim Compass As String * 3   'Compass heading

Main:
    Clear : DelayMS 300
   
    'Load some values for demonstration
    'To convert from SDWord to Decimal Degrees use procedure SIntToDeg
   
    'Destination Point Latitude NMEA: 2852.7780 or 28527780 becomes 28.87963333 degrees
    Lat = SIntToDeg(28527780)
    'Destination Point Longitude NMEA: 2759.0199 or 27590199 becomes 27.983665 degrees
    Lon = SIntToDeg(27590199)
    'Point of origin Latitude NMEA: 2952.7780 or 29527780 becomes 29.87963333 degrees
    LatOrg = SIntToDeg(29527780)
    'Point of origin Longitude NMEA: 2753.0186 or 27530186 becomes 27.88364333 degrees
    LonOrg = SIntToDeg(27530186)
   
   
    'Call GPSDistance procedure, passing the coordinates
    'to calculate the distance between the point of origin and the destination point
    'Distance = 110663 meters
    Distance = GPSDistance(Lat, Lon, LatOrg, LonOrg)
   
    'Call GPSBearing procedure, passing the coordinates
    'to calculate the bearing between the point of origin and the destination point
    'Bearing = 175 degrees
    Bearing = GPSBearing(Lat, Lon, LatOrg, LonOrg)
   
    'Call BearingToCompass procedure, passing the Bearing as degrees
    'to return the heading in compass terms
    'Compass = " S "
     Compass = BearingToCompass(Bearing)
   
End
   

'This proceudre will convert the NMEA coordinates from SDword To Decimal Degrees     
Proc SIntToDeg (Coordinate As SDword), Float
    Dim TempFloat0 As Float
    Dim TempFloat1 As Float
    Dim TempWord As Word
    Dim TempDWord0 As Dword
    Dim TempDWord1 As Dword
   
    TempDWord0 = Abs(Coordinate)
    TempFloat0 = TempDWord0 / 1000000
    TempWord = TempFloat0
   
    TempDWord0 = Abs(Coordinate)
    TempDWord1 = TempDWord0 // 1000000
    TempFloat0 = TempDWord1 / 600000
   
    TempFloat1 = TempWord + TempFloat0
   
    If Coordinate < 0 Then
        Result = -TempFloat1
    Else
        Result = TempFloat1
    EndIf
EndProc   

'This procedure converts degrees to radians
Proc DegToRad (Degree As Float), Float
    Dim TempFloat0 As Float
   
    TempFloat0 = Degree * 0.0174532925199
   
    Result = TempFloat0
EndProc

'This procedure calculates the distance, given the
'coordinates of the destination and the point of origin
'returning the distance as 32b integer in meters
Proc GPSDistance (Lat As Float, Lon As Float, LatOrg As Float, LonOrg As Float), Dword
    Dim TempFloat0 As Float
    Dim TempFloat1 As Float
    Dim TempFloat2 As Float
    Dim TempFloat3 As Float
   
    TempFloat0 = Lat - LatOrg               
   
    TempFloat1 = Lon - LonOrg
    TempFloat2 = DegToRad(LatOrg)
    TempFloat3 = Cos(TempFloat2)           
    TempFloat2 = TempFloat1 * TempFloat3   
    TempFloat1 = TempFloat0 * TempFloat0   
    TempFloat0 = TempFloat2 * TempFloat2   
    TempFloat2 = TempFloat1 + TempFloat0   
    TempFloat0 = Sqr(TempFloat2)
    TempFloat1 = 110250 * TempFloat0 
   
    Result = TempFloat1
EndProc

'This procedure calculates the bearing, given the
'coordinates of the destination and the point of origin
'returning the bearing as 16b integer in degrees
Proc GPSBearing (Lat As Float, Lon As Float, LatOrg As Float, LonOrg As Float), Word
    Dim TempFloat0 As Float
    Dim TempFloat1 As Float
    Dim TempFloat2 As Float
    Dim TempFloat3 As Float
   
    TempFloat0 = Lat / 2
    TempFloat1 = TempFloat0 + 45
    TempFloat0 = DegToRad(TempFloat1)       
   
    TempFloat1 = LatOrg / 2
    TempFloat2 = TempFloat1 + 45
    TempFloat1 = DegToRad(TempFloat2)       
   
    TempFloat2 = Tan(TempFloat0)           
    TempFloat0 = Tan(TempFloat1)           
    TempFloat1 = TempFloat2 / TempFloat0
    TempFloat0 = Log(TempFloat1)            'x
   
    TempFloat3 = DegToRad(Lon)
    TempFloat2 = DegToRad(LonOrg)
    TempFloat1 = TempFloat3 - TempFloat2    'y
   
    'atan2(y,x)
    If TempFloat0 > 0 Then                              'x > 0
        TempFloat3 = TempFloat1 / TempFloat0
        TempFloat2 = ATan(TempFloat3)
    ElseIf TempFloat0 < 0 And TempFloat1 >= 0  Then     'x < 0 and y >= 0
        TempFloat2 = TempFloat1 / TempFloat0
        TempFloat3 = ATan(TempFloat2)     
        TempFloat2 = TempFloat3 + 3.141592653582
    ElseIf TempFloat0 < 0 And TempFloat1 < 0  Then      'x < 0 and y < 0
        TempFloat2 = TempFloat1 / TempFloat0
        TempFloat3 = ATan(TempFloat2)     
        TempFloat2 = TempFloat3 - 3.141592653582 
    ElseIf TempFloat0 = 0 And TempFloat1 > 0  Then      'x = 0 and y > 0 
        TempFloat2 = 1.570796326791
    ElseIf TempFloat0 = 0 And TempFloat1 < 0  Then      'x = 0 and y < 0 
        TempFloat2 = -1.570796326791 
    ElseIf TempFloat0 = 0 And TempFloat1 = 0  Then      'x = 0 and y = 0 
        TempFloat2 = 0 
    EndIf

    TempFloat0 = TempFloat2 * 57.2957795130
   
    Result = TempFloat0
EndProc


'This procedure will select the heading in terms
'of compass direction, given bearing as degrees
Proc BearingToCompass (Bearing As Word), String * 3
    If Bearing >= 349 Or Bearing < 11 Then
        Result = " N "
    ElseIf Bearing >= 11 And Bearing < 34 Then
        Result = "NNE"
    ElseIf Bearing >= 34 And Bearing < 56 Then
        Result = "NE "
    ElseIf Bearing >= 56 And Bearing < 79 Then
        Result = "ENE"
    ElseIf Bearing >= 79 And Bearing < 101 Then
        Result = " E "
    ElseIf Bearing >= 101 And Bearing < 124 Then
        Result = "ESE"
    ElseIf Bearing >= 124 And Bearing < 146 Then
        Result = "SE "
    ElseIf Bearing >= 146 And Bearing < 169 Then
        Result = "SSE"
    ElseIf Bearing >= 169 And Bearing < 191 Then
        Result = " S "
    ElseIf Bearing >= 191 And Bearing < 214 Then
        Result = "SSW"
    ElseIf Bearing >= 214 And Bearing < 236 Then
        Result = "SW "
    ElseIf Bearing >= 236 And Bearing < 259 Then
        Result = "WSW"
    ElseIf Bearing >= 259 And Bearing < 281 Then
        Result = " W "
    ElseIf Bearing >= 281 And Bearing < 304 Then
        Result = "WNW"
    ElseIf Bearing >= 304 And Bearing < 326 Then
        Result = "NW "
    ElseIf Bearing >= 326 And Bearing < 349 Then
        Result = "NNW"
    EndIf
EndProc