def convert_dec_latlon_to_deg(dec_latitude, dec_longtitude):
latlon_position = LatLon(dec_latitude, dec_longtitude)
latlon_strs = latlon_position.to_string('d% %m% %S% %H')
lat_deg = latlon_strs[0].split()
long_deg = latlon_strs[1].split()
# if ((latlon_position.to_string('d% %m% %S% %H')[1]).find('e') != -1):
# weird bug that put e instead of e- when the sec is very small
# pdb.set_trace()
# print latlon_strs
# print lat_deg
# print long_deg
return [lat_deg, long_deg]
class SpatialState(object):
def __init__(self, lat, lon, alt, heading=0):
self._latlon = LatLon(lat, lon)
self._alt = alt
self._heading = heading
@property
def altitude(self):
return self._alt
@property
def latitude(self):
return self._latlon.lat.decimal_degree
@property
def longitude(self):
return self._latlon.lon.decimal_degree
@property
def heading(self):
return self._heading
def vector(self, waypoint):
"""
:type waypoint: SpatialState
"""
movevec = self._latlon - waypoint._latlon
return SpatialVector(
vector=movevec,
altitude=waypoint._alt - self._alt, )
def move(self, vec):
"""
:type vec: SpacialVector
"""
self._latlon -= vec.vector
self._alt += vec.altitude
self._heading = vec.heading
def __str__(self):
return 'Position ({}) Altitude {}m Heading{} degrees'\
.format(self._latlon.lat(), self._latlon.lon(), self._alt, self._heading)
class SpatialState(object):
def __init__(self, lat, lon, alt, heading=0):
self._latlon = LatLon(lat, lon)
self._alt = alt
self._heading = heading
@property
def altitude(self):
return self._alt
@property
def latitude(self):
return self._latlon.lat.decimal_degree
@property
def longitude(self):
return self._latlon.lon.decimal_degree
@property
def heading(self):
return self._heading
def vector(self, waypoint):
"""
:type waypoint: SpatialState
"""
movevec = self._latlon - waypoint._latlon
return SpatialVector(vector=movevec,
altitude=waypoint._alt - self._alt)
def move(self, vec):
"""
:type vec: SpacialVector
"""
self._latlon -= vec.vector
self._alt += vec.altitude
self._heading = vec.heading
def __str__(self):
return 'Position ({}) Altitude {}m Heading{} degrees'\
.format(self._latlon.lat(), self._latlon.lon(), self._alt, self._heading)
lat_current = lat_past
lon_current = lon_past
#drone_orientation=orientation_past
distance_covered = 0
elif lat_check and lon_check and latp_check and lonp_check: #Normal situation where you have the past and present positions:
lat_current = float(lat_current)
lon_current = float(lon_current)
lat_past = float(lat_past)
lon_past = float(lon_past)
# Compute the driven distance
#Other way (the good one)
current = LatLon(Latitude(lat_current), Longitude(lon_current))
past = LatLon(Latitude(lat_past), Longitude(lon_past))
distance_covered = past.distance(
current, ellipse='sphere') * 1000 # WGS84 distance in m
# If covered distance is really small (we suspect that we didn't move)
if distance_covered < treshold_dist:
drone_orientation = orientation_past
speed = speed_past
valid = 5
else: # If covered distance is NOT small (decent)
speed = (distance_covered / (time_pass / 1000000000)) * 3.6 #In km/h
#valid 6 and 7 already considered
def __init__(self, lat, lon, alt, heading=0):
self._latlon = LatLon(lat, lon)
self._alt = alt
self._heading = heading
