mavextra: fixed distance_two(GPS,XKF1[0])

mavextra.py

@@ -511,10 +511,14 @@ 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'):
+        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
@@ -525,15 +529,9 @@ def get_lat_lon_alt(MSG):
         alt = MSG.alt*0.001
     elif hasattr(MSG, 'PN'):
         # origin relative position from EKF
-        global ORGN
-        if ORGN is None:
-            ORGN = get_origin()
-        if ORGN is None:
-            return None
-        (lat,lon) = gps_offset(ORGN.Lat, ORGN.Lng, MSG.PN, MSG.PE)
+        (lat,lon,alt) = ekf1_pos(MSG)
         lat = radians(lat)
         lon = radians(lon)
-        alt = ORGN.Alt - MSG.PD
     else:
         return None
     return (lat, lon, alt)
@@ -1076,7 +1074,8 @@ def ekf1_pos(EKF1):
       ekf_origin = self.messages['ORGN']
       (ekf_origin.Lat, ekf_origin.Lng) = (ekf_origin.Lat, ekf_origin.Lng)
   (lat,lon) = gps_offset(ekf_origin.Lat, ekf_origin.Lng, EKF1.PE, EKF1.PN)
-  return (lat, lon)
+  alt = ekf_origin.Alt - EKF1.PD
+  return (lat, lon, alt)
 
 def quat_to_euler(q):
   '''