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: cope with more Lat/Lon/Lng combinations #785

Merged
merged 1 commit into from
Mar 19, 2023
Merged

mavextra: cope with more Lat/Lon/Lng combinations #785

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
13 changes: 11 additions & 2 deletions mavextra.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -513,18 +513,26 @@ def get_origin():

def get_lat_lon_alt(MSG):
'''gets lat and lon in radians and alt in meters from a position msg'''
if hasattr(MSG, 'Lat'):
if hasattr(MSG, 'Lat') and hasattr(MSG, 'Lng'):
lat = radians(MSG.Lat)
lon = radians(MSG.Lng)
alt = MSG.Alt
elif hasattr(MSG, 'Lat') and hasattr(MSG, 'Lon'):
lat = radians(MSG.Lat)
lon = radians(MSG.Lon)
alt = MSG.Alt
elif hasattr(MSG, 'cog'):
lat = radians(MSG.lat)*1.0e-7
lon = radians(MSG.lon)*1.0e-7
alt = MSG.alt*0.001
elif hasattr(MSG,'lat'):
elif hasattr(MSG,'lat') and hasattr(MSG,'lon'):
lat = radians(MSG.lat)
lon = radians(MSG.lon)
alt = MSG.alt*0.001
elif hasattr(MSG,'lat') and hasattr(MSG,'lng'):
lat = radians(MSG.lat)
lon = radians(MSG.lng)
alt = MSG.alt*0.001
elif hasattr(MSG, 'PN'):
# origin relative position from EKF
global ORGN
Expand All @@ -540,6 +548,7 @@ def get_lat_lon_alt(MSG):
return None
return (lat, lon, alt)


def _distance_two(MSG1, MSG2, horizontal=True):
'''distance between two points'''
(lat1, lon1, alt1) = get_lat_lon_alt(MSG1)
Expand Down