Calculating the distance and the bearing between two GPS coordinates

[trastikata]

Hello all,

I am sharing my code for the calculation of the distance and the bearing between two GPS coordinates taken from the NMEA message in the hope it can be useful to others. In my original application the Longitude and Latitude coordinates are converted each to 4-byte SDword and transmitted to a remote receiver. For example:

2951.7738,N becomes 29517738
2951.7738,S becomes -29517738
07754.0066,E becomes 77540066
07754.0066,W becomes -77540066

To accelerate the conversion and to avoid loss of accuracy due to limitations of 32b floating point math, some of the calculations are using "scaled-up" integers. Also the formulae are broken to smaller chunks in order to avoid overflows. For further acceleration I've pre-calculated some coefficients. I have the bad habit of not using much commenting but the names of the procedures and the variables are mostly self-explanatory.

In my code the Latitude and Longitude coordinates (as SDword integers) are converted from degrees to radians and into smaller temporary variables, then the distance between the origin and the current position is calculates, then the bearing, and finally the bearing as heading is selected.

Code Select Expand

'Temporary variables
Dim Temp_Float As Float
Dim Temp_Float1 As Float
Dim Temp_Float2 As Float
Dim Temp_Float3 As Float
Dim Temp_Float4 As Float
Dim Temp_Float5 As Float
Dim Temp_Float6 As Float
Dim Temp_Float7 As Float
Dim Temp_Float8 As Float
Dim Temp_Float9 As Float
Dim Temp_Float10 As Float
Dim tempdword As Dword
Dim tempdword1 As Dword
Dim temp_byte As Byte
Dim tempsdword As SDword
Dim tempsdword1 As SDword
Dim tempsdword2 As SDword 

'Partial Coordinates in Degrees converted to integers
Dim deg_lat_sw As SWord
Dim deg_lat_sd As SDword
Dim deg_lon_sw As SWord
Dim deg_lon_sd As SDword
Dim deg_lat_sw_Org As SWord
Dim deg_lat_sd_Org As SDword
Dim deg_lon_sw_Org As SWord
Dim deg_lon_sd_Org As SDword

'Partial Coordinates in Degrees converted to radians
Dim Rad_Lat As Float
Dim Rad_Lon As Float
Dim Rad_Lat_Org As Float
Dim Rad_Lon_Org As Float

'Original Coordinates in Degrees converted to integers
'NMEA coordinates are received as single DWord integeres
'For example 2951.7738,N becomes 29517738
'For example 2951.7738,S becomes -29517738
'For example 07754.0066,E becomes 77540066
'For example 07754.0066,W becomes -77540066
'Negative is for South or West
Dim Lat As SDword
Dim Lon As SDword

'Final results
Dim Distance As Dword
Dim Bearing As SWord
Dim Bearing_String As String * 3

MAIN:
    GoSub DEGREE_LAT_SUB
    GoSub DEGREE_LON_SUB
    GoSub CALCULATE_DISTANCE
    GoSub FIND_BEARING
    GoSub SELECT_BEARING
   
    GoTo MAIN


'=============================================================================================
'GPS coordinates are broken into degrees and minutes
'And then converted to radians but using large integeres
'To avoid loss of accuracy when using 32-bit floating point math
'Also to accelrate calculations all math is with integers
DEGREE_LAT_SUB:
    deg_lat_sw = Lat / 1000000
    deg_lat_sd = 0
    temp_byte = Dig Lat, 5
    tempsdword = temp_byte * 100000
    deg_lat_sd = deg_lat_sd + tempsdword
    temp_byte = Dig Lat, 4
    tempsdword = temp_byte * 10000
    deg_lat_sd = deg_lat_sd + tempsdword
    temp_byte = Dig Lat, 3
    tempsdword = temp_byte * 1000
    deg_lat_sd = deg_lat_sd + tempsdword
    temp_byte = Dig Lat, 2
    tempsdword = temp_byte * 100
    deg_lat_sd = deg_lat_sd + tempsdword
    temp_byte = Dig Lat, 1
    tempsdword = temp_byte * 10
    deg_lat_sd = deg_lat_sd + tempsdword
    temp_byte = Dig Lat, 0
    tempsdword = temp_byte * 1
    deg_lat_sd = deg_lat_sd + tempsdword
    If deg_lat_sw >= 0 Then
        tempsdword = deg_lat_sd * 1000
        deg_lat_sd = tempsdword / 6
    Else
        tempsdword = deg_lat_sd * (-1000)
        deg_lat_sd = tempsdword / 6
    EndIf
Return 

DEGREE_LON_SUB:
    deg_lon_sw = Lon / 1000000
    deg_lon_sd = 0
    temp_byte = Dig Lon, 5
    tempsdword = temp_byte * 100000
    deg_lon_sd = deg_lon_sd + tempsdword
    temp_byte = Dig Lon, 4
    tempsdword = temp_byte * 10000
    deg_lon_sd = deg_lon_sd + tempsdword
    temp_byte = Dig Lon, 3
    tempsdword = temp_byte * 1000
    deg_lon_sd = deg_lon_sd + tempsdword
    temp_byte = Dig Lon, 2
    tempsdword = temp_byte * 100
    deg_lon_sd = deg_lon_sd + tempsdword
    temp_byte = Dig Lon, 1
    tempsdword = temp_byte * 10
    deg_lon_sd = deg_lon_sd + tempsdword
    temp_byte = Dig Lon, 0
    tempsdword = temp_byte * 1
    deg_lon_sd = deg_lon_sd + tempsdword
    If deg_lon_sw >= 0 Then
        tempsdword = deg_lon_sd * 1000
        deg_lon_sd = tempsdword / 6
    Else
        tempsdword = deg_lon_sd * (-1000)
        deg_lon_sd = tempsdword / 6
    EndIf 
Return

'=============================================================================================
'Calculate distance between two GPS coordinates in meters
'Variables with Org in it refer to the point of origin
CALCULATE_DISTANCE:
    'Dy
    tempsdword = deg_lat_sw_Org - deg_lat_sw
    tempsdword1 = deg_lat_sd_Org - deg_lat_sd
    Temp_Float = tempsdword1 * 0.1111342
    Temp_Float4 = Temp_Float / 100
    Temp_Float3 = tempsdword + Temp_Float4
    Temp_Float = Temp_Float3 * Temp_Float3
    'Dx
    tempsdword = deg_lon_sw_Org - deg_lon_sw
    tempsdword1 = deg_lon_sd_Org - deg_lon_sd
    Temp_Float3 = tempsdword1 * 0.1113174
    Temp_Float4 = Temp_Float3 / 100
    Temp_Float1 = tempsdword + Temp_Float4
    'cos
    tempsdword2 = deg_lat_sw_Org + deg_lat_sw
    tempsdword = tempsdword2 / 2
    tempsdword2 = deg_lat_sd_Org + deg_lat_sd
    tempsdword1 = tempsdword2 / 2
     
    tempsdword2 = tempsdword * 1000000
    tempsdword = tempsdword2 / 113
    tempsdword2 = tempsdword * 355
    tempsdword = tempsdword2 / 180
    Temp_Float2 = tempsdword / 1000000
     
    tempsdword = tempsdword1 / 113
    tempsdword1 = tempsdword * 355
    tempsdword = tempsdword1 / 180
    Temp_Float3 = tempsdword / 1000000000
    Temp_Float4 = Temp_Float2 + Temp_Float3
    Temp_Float2 = Cos(Temp_Float4)
    'Dx
    Temp_Float4 = Temp_Float1 * Temp_Float2
    Temp_Float1 = Temp_Float4 * Temp_Float4
    'Distance
    Temp_Float4 = Temp_Float + Temp_Float1
    Temp_Float = Sqr(Temp_Float4)
    Distance = Temp_Float
Return

'=============================================================================================
'Find the bearing between two GPS coordinates expressed in 0 to 360 degrees
'Variables with Org in it refer to the point of origin
FIND_BEARING:
    tempsdword = deg_lat_sw
    tempsdword1 = deg_lat_sd
    tempsdword2 = tempsdword * 1000000
    tempsdword = tempsdword2 / 113
    tempsdword2 = tempsdword * 355
    tempsdword = tempsdword2 / 180
    Temp_Float2 = tempsdword / 1000000
    tempsdword = tempsdword1 / 113
    tempsdword1 = tempsdword * 355
    tempsdword = tempsdword1 / 180
    Temp_Float3 = tempsdword / 100000000
    Rad_Lat = Temp_Float2 + Temp_Float3
   
    tempsdword = deg_lon_sw
    tempsdword1 = deg_lon_sd
    tempsdword2 = tempsdword * 1000000
    tempsdword = tempsdword2 / 113
    tempsdword2 = tempsdword * 355
    tempsdword = tempsdword2 / 180
    Temp_Float2 = tempsdword / 1000000
    tempsdword = tempsdword1 / 113
    tempsdword1 = tempsdword * 355
    tempsdword = tempsdword1 / 180
    Temp_Float3 = tempsdword / 100000000
    Rad_Lon = Temp_Float2 + Temp_Float3
   
    tempsdword = deg_lat_sw_Org
    tempsdword1 = deg_lat_sd_Org
    tempsdword2 = tempsdword * 1000000
    tempsdword = tempsdword2 / 113
    tempsdword2 = tempsdword * 355
    tempsdword = tempsdword2 / 180
    Temp_Float2 = tempsdword / 1000000
    tempsdword = tempsdword1 / 113
    tempsdword1 = tempsdword * 355
    tempsdword = tempsdword1 / 180
    Temp_Float3 = tempsdword / 100000000
    Rad_Lat_Org = Temp_Float2 + Temp_Float3
   
    tempsdword = deg_lon_sw_Org
    tempsdword1 = deg_lon_sd_Org
    tempsdword2 = tempsdword * 1000000
    tempsdword = tempsdword2 / 113
    tempsdword2 = tempsdword * 355
    tempsdword = tempsdword2 / 180
    Temp_Float2 = tempsdword / 1000000
    tempsdword = tempsdword1 / 113
    tempsdword1 = tempsdword * 355
    tempsdword = tempsdword1 / 180
    Temp_Float3 = tempsdword / 100000000
    Rad_Lon_Org = Temp_Float2 + Temp_Float3

    'lon2-lon1
    tempsdword = deg_lon_sw - deg_lon_sw_Org
    tempsdword1 = deg_lon_sd - deg_lon_sd_Org
    tempsdword2 = tempsdword * 1000000
    tempsdword = tempsdword2 / 113
    tempsdword2 = tempsdword * 355
    tempsdword = tempsdword2 / 180
    Temp_Float2 = tempsdword / 1000000
    tempsdword = tempsdword1 / 113
    tempsdword1 = tempsdword * 355
    tempsdword = tempsdword1 / 180
    Temp_Float3 = tempsdword / 100000000
    Temp_Float4 = Temp_Float2 + Temp_Float3
    Temp_Float3 = Temp_Float4  'lon2-lon1
   
    Temp_Float4 = Sin(Temp_Float3) 'sin(lon2-lon1)
    Temp_Float5 = Cos(Rad_Lat) 'cos(lat2)
    Temp_Float = Temp_Float4 * Temp_Float5   'y
   
    Temp_Float5 = Sin(Rad_Lat_Org)      'sin(lat1)
    Temp_Float6 = Cos(Rad_Lat)      'cos(lat2)
    Temp_Float7 = Cos(Temp_Float3)  'cos(lon2-lon1)
    Temp_Float8 = Cos(Rad_Lat_Org)      'cos(lat1)
    Temp_Float9 = Sin(Rad_Lat)      'sin(lat2)
   
    Temp_Float1 = Temp_Float8 * Temp_Float9  'cos(lat1)*sin(lat2)
    Temp_Float2 = Temp_Float5 * Temp_Float6  'sin(lat1)*cos(lat2)
    Temp_Float10 = Temp_Float2 * Temp_Float7 'sin(lat1)*cos(lat2)*cos(lon2-lon1)
    Temp_Float2 = Temp_Float1 - Temp_Float10
    Temp_Float1 = Temp_Float2                'x
   
    'atan2
    Select Temp_Float1
        Case = 0
            Select Temp_Float
                Case > 0
                    Temp_Float10 = 180
                Case <= 0
                    Temp_Float10 = -180
            EndSelect             
        Case > 0
            Temp_Float2 = Temp_Float / Temp_Float1   'y/x
            Temp_Float9 = ATan(Temp_Float2) 'arctan(y/x)
            Temp_Float10 = Temp_Float9 * 57.296
        Case < 0
            Select Temp_Float
                Case >= 0
                    Temp_Float2 = Temp_Float / Temp_Float1   'y/x
                    Temp_Float9 = ATan(Temp_Float2) 'arctan(y/x)
                    Temp_Float8 = Temp_Float9 + 3.14159265358979
                    Temp_Float10 = Temp_Float8 * 57.296
                Case < 0
                    Temp_Float2 = Temp_Float / Temp_Float1   'y/x
                    Temp_Float9 = ATan(Temp_Float2) 'arctan(y/x)
                    Temp_Float8 = Temp_Float9 - 3.14159265358979
                    Temp_Float10 = Temp_Float8 * 57.296
            EndSelect                     
    EndSelect
   
    Bearing = Temp_Float10
    If Bearing >= 0 Then
        Bearing = Bearing // 360
    Else
        Bearing = Bearing // 360
        Bearing = 360 - Bearing
    EndIf       
Return


'=============================================================================================
'Select bearing as string based on the heading calculated in degrees
SELECT_BEARING:
    If Bearing >= 349 Or Bearing < 11 Then
        Bearing_String = "  N"
    ElseIf Bearing >= 11 And Bearing < 34 Then
        Bearing_String = "NNE"
    ElseIf Bearing >= 34 And Bearing < 56 Then
        Bearing_String = " NE"
    ElseIf Bearing >= 56 And Bearing < 79 Then
        Bearing_String = "ENE"
    ElseIf Bearing >= 79 And Bearing < 101 Then
        Bearing_String = "  E"
    ElseIf Bearing >= 101 And Bearing < 124 Then
        Bearing_String = "ESE"
    ElseIf Bearing >= 124 And Bearing < 146 Then
        Bearing_String = " SE"
    EndIf 
   
    If Bearing >= 146 And Bearing < 169 Then
        Bearing_String = "SSE"
    ElseIf Bearing >= 169 And Bearing < 191 Then
        Bearing_String = "  S"
    ElseIf Bearing >= 191 And Bearing < 214 Then
        Bearing_String = "SSW"
    ElseIf Bearing >= 214 And Bearing < 236 Then
        Bearing_String = " SW"
    ElseIf Bearing >= 236 And Bearing < 259 Then
        Bearing_String = "WSW"
    ElseIf Bearing >= 259 And Bearing < 281 Then
        Bearing_String = "  W"
    ElseIf Bearing >= 281 And Bearing < 304 Then
        Bearing_String = "WNW"
    ElseIf Bearing >= 304 And Bearing < 326 Then
        Bearing_String = " NW"
    ElseIf Bearing >= 326 And Bearing < 349 Then
        Bearing_String = "NNW"
    EndIf   
Return

'=============================================================================================


[John Drew]

Well done Trastikata.
Using integers can overcome some of the disadvantages of 32 bit floats. You've done well to do it with integers.
Did you consider including altitude effects for short distances? Though it all depends how accurate you want your solution.
It would be helpful for users if you put up a working program in the WIKI otherwise it gets lost in the forum.
Best wishes
John

[trastikata]

Thank you John.

At good signal reception, the elevation accuracy is about 20-30 meters, when the signal is not so good, the deviation in the elevation readings can rise up to 50-60 meters, therefore I decided not to use the GPS elevation readings.

However since the attached code calculates the distance between two points as projection on the earth's surface, if the 3D distance is required, then as an approximation, I think solving a simple Pythagorean triangle to find the hypotenuse should suffice. 

I can post a working program on WIKI but first I'll have to comment out most of the code, this could take some time.

Regards.

[John Drew]

All noted Trastikata. The equations I've used in the past fall down over short distances because the trig functions are not sufficiently accurate in a PIC so the errors become a large portion of the result. So I switch over to Pythagoras like you do.
You have been more creative using integers. A speedier and probably more accurate way to do things.
Best wishes
John