Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

mavextra: correct radian conversion in distance_lat_lon function #903

Merged
merged 1 commit into from
Jan 12, 2024
Merged

mavextra: correct radian conversion in distance_lat_lon function #903

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
33 changes: 27 additions & 6 deletions mavextra.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -549,7 +549,13 @@ def get_lat_lon_alt(MSG):


def _distance_two(MSG1, MSG2, horizontal=True):
'''distance between two points'''
'''
Return the distance between two points in metres
Calculated as the great-circle distance using 'haversine’ formula
(Ref: http://www.movable-type.co.uk/scripts/latlong.html)
Uses the globally-average earth radius value of 6371km
'''
# get_lat_lon_alt returns radians - so no need to convert here
(lat1, lon1, alt1) = get_lat_lon_alt(MSG1)
(lat2, lon2, alt2) = get_lat_lon_alt(MSG2)
dLat = lat2 - lat1
Expand Down Expand Up @@ -1044,7 +1050,7 @@ def distance_gps2(GPS, GPS2):
return None
return distance_two(GPS, GPS2)


# SI base unit for 1 Rgeo / equatorial radius
radius_of_earth = 6378100.0 # in meters

def wrap_valid_longitude(lon):
Expand Down Expand Up @@ -1499,7 +1505,12 @@ def airspeed_estimate(GLOBAL_POSITION_INT,WIND):


def distance_from(GPS_RAW1, lat, lon):
'''distance from a given location'''
'''
Return the distance from a given location in meters
Calculated as the great-circle distance using 'haversine’ formula
(Ref: http://www.movable-type.co.uk/scripts/latlong.html)
Uses the globally-average earth radius value of 6371km
'''
if hasattr(GPS_RAW1, 'Lat'):
lat1 = radians(GPS_RAW1.Lat)
lon1 = radians(GPS_RAW1.Lng)
Expand All @@ -1522,9 +1533,19 @@ def distance_from(GPS_RAW1, lat, lon):
return ground_dist

def distance_lat_lon(lat1, lon1, lat2, lon2):
'''distance between two points'''
dLat = radians(lat2) - radians(lat1)
dLon = radians(lon2) - radians(lon1)
'''
Return the distance between two points in metres
Calculated as the great-circle distance using 'haversine’ formula
(Ref: http://www.movable-type.co.uk/scripts/latlong.html)
Uses the globally-average earth radius value of 6371km
'''
lat1 = radians(lat1)
lon1 = radians(lon1)
lat2 = radians(lat2)
lon2 = radians(lon2)

dLat = lat2 - lat1
dLon = lon2 - lon1

a = sin(0.5*dLat)**2 + sin(0.5*dLon)**2 * cos(lat1) * cos(lat2)
c = 2.0 * atan2(sqrt(a), sqrt(1.0-a))
Expand Down
Loading