def convertToObstaclePolygons(self, numVertices=8):
'''
Accepts a list of obstacle dictionaries and returns a list of mavsdk compatible
obstacle polygons
'''
# Initialize list to hold obstacle polygons
obstaclePolygonList = []
for obstacle in self.obstacles:
# Initialize list to hold points in each obstacle polygon
obstaclePointList = []
# Capture data from the current obstacle
latitude = obstacle["latitude"]
longitude = obstacle["longitude"]
radius = self.ConvertFootToMeter(obstacle["radius"])
# Initialize ellipsoidal Vincenty point
obstaclePoint = LatLon(latitude, longitude)
# For each vertex point in the polygon that represents the circular obstacle
for index in range(numVertices):
# Calculate the azimuth angle of the vertex
angle = index * 360 / numVertices
# Calculate the gps coordinate of the vertex based off the obstacles
# radius and azimuth angle
vertex = obstaclePoint.destination(radius, angle)
vertexPoint = Point(float(vertex.lat), float(vertex.lon))
obstaclePointList.append(vertexPoint)
# Append the current obstacle polygon to the obstaclePolygonList
obstaclePolygon = Polygon(obstaclePointList,
Polygon.FenceType.EXCLUSION)
obstaclePolygonList.append(obstaclePolygon)
return obstaclePolygonList
def lat_lon_from_reference_given_range_az_degrees(ref_lat, ref_lon,
range_nmi,
az_degrees) -> list:
p = LatLon(ref_lat, ref_lon)
range_meters = range_nmi * constants.NM_TO_METERS
d = p.destination(range_meters, az_degrees)
return (d.lat, d.lon)
def update(self, fromWho, val):
#print("update from[",fromWho,"] ->",val)
if fromWho == 'gps':
if self.iter == 0:
self.lastT = self.th.getTimestamp(microsec=True)
self.lastVal = val
self.lastLL = LatLon(val['lat'], val['lon'])
else:
t = self.th.getTimestamp(microsec=True)
ll = LatLon(val['lat'], val['lon'])
dis = ll.distanceTo(self.lastLL)
tSince = (self.lastT - t) / 1000000.0
dis = ((dis / 1000.00) / tSince)
if dis < 0.0:
dis = -dis
self.data['total'] += dis
self.data['trip'] += dis
self.data['iters'] = self.iter
self.data['total iters'] += 1
self.lastT = t
self.lastVal = val
self.lastLL = ll
#if self.gui.isReady:
# # callbacks
# for o in self.callBacksForUpdate:
# o.update(self.type, self.data)
self.broadcastCallBack(self.gui, self.type, self.data)
self.iter += 1
def detection_map_2(map_det_obj_str1: str, map_det_obj_str2: str,
build_map: bool):
"""
Detection map function for two images.
:param map_det_obj_str1: String representing first MapDetectionObj. This one is used as zero point.
:param map_det_obj_str2: String representing a second MapDetectionObj
:param build_map: Pass 1 if a map should be created, 0 otherwise (booleans are not supported yet)
:return: Dict containing the names and the GPS-coordinates of all detected object
"""
# image from car 1
map_det_obj1 = DetectionMapObject.from_string(map_det_obj_str1)
image1, lat1, lon1, start_bearing1, scaling_factor_z1, scaling_factor_x1 = map_det_obj1.unpack(
)
start_point1 = LatLon(lat1, lon1)
# image from car 2
map_det_obj2 = DetectionMapObject.from_string(map_det_obj_str2)
image2, lat2, lon2, start_bearing2, scaling_factor_z2, scaling_factor_x2 = map_det_obj2.unpack(
)
start_point2 = LatLon(lat2, lon2)
d = DetectedObjectGPS()
gps = d.run(start_point1, start_bearing1, image1, scaling_factor_z1,
scaling_factor_x1, 0)
gps += d.run(start_point2, start_bearing2, image2, scaling_factor_z2,
scaling_factor_x2, 1)
x, z = d.gps_to_global_xz(start_point1, start_bearing1, gps)
d.plot_data(x, z)
if build_map:
d.create_map(lat1, lon1)
return d.classified_map_detections
def canonicalize_line(line):
"""Ensures a consistent order of a line's coordinates."""
coord1 = LatLon(line.coords[0][1], line.coords[0][0])
coord2 = LatLon(line.coords[1][1], line.coords[1][0])
if bearing_to(coord1, coord2) > 180:
return LineString(reversed(line.coords))
else:
return line
def set_ens(self, ens: Ensemble):
"""
Get the data out of the ensemble and populate
the structure.
Lock and unlock when getting the data to ensure
when new data is received, it is not being used with
old data.
:param ens: Latest ensemble.
:return:
"""
# Lock the object
self.thread_lock.acquire()
# Set Data
if ens.IsNmeaData and ens.NmeaData.GPGGA is not None and ens.IsEarthVelocity:
# Lat and Lon to the arrays
self.lat.append(ens.NmeaData.latitude)
self.lon.append(ens.NmeaData.longitude)
#self.lat_lon_text.append("Lat: " + str(round(ens.NmeaData.latitude, 2)) + " Lon: " + str(round(ens.NmeaData.longitude, 2)))
# Last Lat/Lon to place a marker for end of the path
self.last_lat = ens.NmeaData.latitude
self.last_lon = ens.NmeaData.longitude
# Get the average velocity and direction for the ensemble
avg_mag, avg_dir = ens.EarthVelocity.average_mag_dir()
self.avg_mag.append(avg_mag)
self.avg_dir.append(avg_dir)
# Create the magnitude and direction line
# Use the given Lat/Lon position as the start point
position = LatLon(ens.NmeaData.latitude, ens.NmeaData.longitude)
# Calculate the new position, based on the current lat/lon and the avg mag and dir
# These 2 points will create the quiver that represents the mag and dir for the given
# latitude point
if not math.isnan(avg_mag) and not math.isnan(avg_dir):
avg_position = position.destination(avg_mag * self.mag_scale,
avg_dir)
# Add the points to the arrays
# The start point is the ship track line
# The end point is the magnitude and angle from the start point on the ship track line
self.quiver_x.append(position.lon)
self.quiver_y.append(position.lat)
self.quiver_x.append(avg_position.lon)
self.quiver_y.append(avg_position.lat)
self.quiver_x.append(
None) # Add a None to breakup the lines for each quiver
self.quiver_y.append(
None) # Add a None to breakup the lines for each quiver
self.quiver_text.append("Mag: " + str(round(avg_mag, 2)) +
" Dir: " + str(round(avg_dir, 2)))
# Release the lock
self.thread_lock.release()
def getroutedistance(host_latitude, host_longitude):
latitude = list(host_latitude)
longitude = list(host_longitude)
distance = []
distance.append(0)
for i in range(0, len(latitude) - 1):
source = LatLon(latitude[i], longitude[i])
destination = LatLon(latitude[i + 1], longitude[i + 1])
distance.append(m2km(source.distanceTo(destination)))
return distance
def lat_lon_from_reference_given_range_az_degrees(self, ref_lat, ref_lon, range_nmi, az_degrees):
"""
:param ref_lat: in format +/- hh.nnnnnn
:param ref_lon: in format +/- hh.nnnnnn
:param range_nmi:
:param az_degrees:
:return: (+/- hh.nnnn, +/- hh.nnnn)
"""
p = LatLon(ref_lat, ref_lon)
range_meters = range_nmi * self.__meters_in_nmi
d = p.destination(range_meters, az_degrees)
return (d.lat, d.lon)
def find_nearest_sensors(debug, max_count, max_distance_km, output_file,
latitude, longitude):
resp = requests.get('https://www.purpleair.com/json')
resp.raise_for_status()
if debug:
with open('debug.json', 'wb') as debug_handle:
debug_handle.write(resp.content)
sensors = resp.json()['results']
here = LatLon(latitude, longitude)
max_distance_m = max_distance_km * 1000
timestamp = time.time()
min_last_seen = timestamp - 3600
n_candidates = 0
near = []
for sensor in sensors:
if sensor.get('A_H'):
# hardware fault detected
continue
if sensor.get('DEVICE_LOCATIONTYPE') != 'outside':
continue
if sensor['LastSeen'] < min_last_seen:
continue
if 'PM2_5Value' not in sensor:
continue
if 'Lat' not in sensor or 'Lon' not in sensor:
continue
n_candidates += 1
distance_m = here.distanceTo(LatLon(sensor['Lat'], sensor['Lon']))
if distance_m <= max_distance_m:
near.append(
Sensor(
id=sensor['ID'],
label=sensor['Label'],
distance_m=distance_m,
))
sys.stderr.write(
'Found %d nearby outdoor sensors (out of %d candidates).\n' %
(len(near), n_candidates))
nearest = sorted(near, key=lambda x: x.distance_m)[:max_count]
output = {
'timestamp':
timestamp,
'latitude':
latitude,
'longitude':
longitude,
'sensors': [{
'id': x.id,
'label': x.label,
'distance_m': x.distance_m,
} for x in nearest],
}
output_file.write(json.dumps(output, indent=4))
def go_to_bookmark(self, evt):
bookmarks = self._person.map.bookmarks
reprs = []
for mark in bookmarks:
point = LatLon(mark.latitude, mark.longitude)
dist = format_number(distance_between(self._person.position, point), config().presentation.distance_decimal_places)
bearing = format_number((bearing_to(self._person.position, point) - self._person.direction) % 360, config().presentation.angle_decimal_places)
reprs.append(_("{name}: distance {distance} meters, {bearing}° relatively").format(name=mark.name, distance=dist, bearing=bearing))
name, ok = QInputDialog.getItem(self._main_window, _("Data entry"), _("Select a bookmark"), reprs, editable=False)
if not ok:
return
bookmark = bookmarks[reprs.index(name)]
self._person.move_to(LatLon(bookmark.latitude, bookmark.longitude), force=True)
self._person.set_direction(bookmark.direction)
def distance_rangeAzDeg_between_two_lat_lon(self, lat1, lon1, lat2, lon2):
"""
:param lat1: in format +/- hh.nnnn
:param lon1: in format +/- hh.nnnn
:param lat2: in format +/- hh.nnnn
:param lon2: in format +/- hh.nnnn
:return: (range_nmi, azDegrees)
"""
p = LatLon(lat1, lon1)
q = LatLon(lat2, lon2)
range_meters = p.distanceTo(q)
range_nmi = range_meters / self.__meters_in_nmi
azDegrees = p.initialBearingTo(q)
return (range_nmi, azDegrees)
def calculate_distance(fix1, fix2):
"""
Calculate distance between fix1 and fix2 using WGS84
:param fix1: b-record from IGC file (dict with keys 'lat' and 'lon')
:param fix2: b-record from IGC file (dict with keys 'lat' and 'lon')
:return: distance in m
"""
loc1_lat_lon = LatLon(fix1['lat'], fix1['lon'])
loc2_lat_lon = LatLon(fix2['lat'], fix2['lon'])
# pygeodesy raises exception when same locations are used
if isclose(fix1['lat'], fix2['lat']) and isclose(fix1['lon'], fix2['lon']):
return 0
return loc1_lat_lon.distanceTo(loc2_lat_lon)
def distance(city1, city2):
print('processing distance for: ', city1, city2, '\n')
# saving coordinates to new variables
coordinates_city1 = get_coordinates(city1)
coordinates_city2 = get_coordinates(city2)
# using LatLon to find distance, convert our list of coordinated to float
start_city = LatLon(float(coordinates_city1[0]),
float(coordinates_city1[1]))
end_city = LatLon(float(coordinates_city2[0]), float(coordinates_city2[1]))
# using distanceTo method of LatLon to find distance in meters
dist = start_city.distanceTo(end_city)
# converting number to float, divide to 1000 because we got number in meters,
# rounding it and converting to int
newdist = int(round(float(dist)) / 1000)
return newdist
def calculate_bearing(fix1, fix2, final_bearing=False):
"""
Calculate bearing between fix1 and fix. By default the bearing is taking tangent to the great circle at fix1.
:param final_bearing: switch to True results in taking the tangent at fix2.
:param fix1: b-record from IGC file (dict with keys 'lat' and 'lon')
:param fix2: b-record from IGC file (dict with keys 'lat' and 'lon')
:return: bearing in degrees
"""
loc1_lat_lon = LatLon(fix1['lat'], fix1['lon'])
loc2_lat_lon = LatLon(fix2['lat'], fix2['lon'])
if not final_bearing:
return loc1_lat_lon.initialBearingTo(loc2_lat_lon)
else:
return loc1_lat_lon.finalBearingTo(loc2_lat_lon)
def get_gps_of_detected_objects(self, image, position: LatLon,
start_bearing, disparity_map, detections,
scaling_factor_z, scaling_factor_x):
"""
Use the GPS coordinates of the camera (car) and the disparity map to calculate the GPS coordinates of the
detected objects.
:param image: The input image
:param position: LatLon object (latitude, longitude) of the position of the camera (car)
:param start_bearing: The viewing direction of the camera as the deviation from north in degrees
:param disparity_map: The disparity map as an image
:param detections: List of dicts containing the name and the x, y-coordinates of the classified objects
:param scaling_factor_z: A factor which scales x and z distances
:return: The GPS-coordinates of the detected objects
"""
original_width = image.shape[1]
gps = []
for detection in detections:
u, v = detection["coords"]
z = self.basline * self.focal_length / (disparity_map[v, u] *
self.training_width)
z *= scaling_factor_z
x = (u - original_width / 2) * z / (
original_width / self.training_width) / self.focal_length
x *= scaling_factor_x
distance, bearing = self.xz_to_dist_bearing(x, z, start_bearing)
new_gps = position.destination(distance, bearing)
gps.append(new_gps)
self.classified_map_detections.append(
(detection.get("id"), new_gps.lat, new_gps.lon))
return gps
def disPrjCalculatePointToEnvelope(pointLat, pointLon, envelopeTuple):
'''
计算给定点到四至中心点的距离，用到投影坐标系CGCS2000
:param pointLat: 输入待测点纬度
:param pointLon: 输入待测点经度
:param envelopeTuple: 四至的元祖，前两个为经度，后两个为纬度，从小到大排序
:return: 投影后计算的距离
'''
centerEnvelopePrj = LatLon(pointLat,
pointLon,
datum=pygeodesy.datum.Datums.CGCS2000)
geom3DEnvelopCenPrj = LatLon((envelopeTuple[2] + envelopeTuple[3]) / 2, \
(envelopeTuple[0] + envelopeTuple[1]) / 2, \
datum=pygeodesy.datum.Datums.CGCS2000)
deviationValue = centerEnvelopePrj.distanceTo(geom3DEnvelopCenPrj)
return deviationValue
def do_jump(self, evt):
x, ok = QInputDialog.getDouble(self._main_window, _("Coordinate"), _("Enter the longitude"), decimals=config().presentation.coordinate_decimal_places, minValue=-180, maxValue=180, value=self._person.position.lon)
if not ok:
return
y, ok = QInputDialog.getDouble(self._main_window, _("Coordinate"), _("Enter the latitude"), decimals=config().presentation.coordinate_decimal_places, minValue=-90, maxValue=90, value=self._person.position.lat)
if not ok:
return
self._person.move_to(LatLon(y, x), force=True)
def xy2dist(x, y, cyclic=False, datum='WGS84'):
"""
USAGE
-----
d = xy2dist(x, y, cyclic=False, datum='WGS84')
"""
if datum is not 'Sphere':
xy = [LatLon(y0, x0, datum=Datums[datum]) for x0, y0 in zip(x, y)]
else:
xy = [LatLon_sphere(y0, x0) for x0, y0 in zip(x, y)]
d = np.array([xy[n].distanceTo(xy[n + 1]) for n in range(len(xy) - 1)])
return np.append(0, np.cumsum(d))
def xy2dist(x, y, cyclic=False, datum='WGS84'):
"""
USAGE
-----
d = xy2dist(x, y, cyclic=False, datum='WGS84')
Calculates a distance axis from a line defined by longitudes and latitudes
'x' and 'y', using either the Vicenty formulae on an ellipsoidal earth
(ellipsoid defaults to WGS84) or on a sphere (if datum=='Sphere').
"""
if datum is not 'Sphere':
xy = [LatLon(y0, x0, datum=Datums[datum]) for x0, y0 in zip(x, y)]
else:
xy = [LatLon_sphere(y0, x0) for x0, y0 in zip(x, y)]
d = np.array([xy[n].distanceTo(xy[n + 1]) for n in range(len(xy) - 1)])
return np.append(0, np.cumsum(d))
def gps_to_global_xz(self, start_point: LatLon, start_bearing, gps):
"""
In order to plot the detected objects in relation to one camera, we need to translate GPS-coordinates
to x, z-coordinates using one camera as the zero point.
:param start_point: LatLon object (latitude, longitude) of the camera defined as the zero point
:param start_bearing: The deviation from north in degrees, determines to direction of the z axis
:param gps: List of GPS-coordinates
:return: The calculated x and z values
"""
x_list, z_list = [], []
for coord in gps:
dist, bearing, _ = start_point.distanceTo3(coord)
bearing_diff = (start_bearing - bearing)
x = -np.sin(np.deg2rad(bearing_diff)) * dist
z = np.cos(np.deg2rad(bearing_diff)) * dist
x_list.append(x), z_list.append(z)
return x_list, z_list
def storm_distance_bearing(point_a: pg.LatLon, point_b: pg.LatLon) -> list:
"""Computes and outputs the distance the storm moved between point_a and point_b
:param point_a: A LatLon object defined in pygeodesy package. First parameter is Latitude and second is Longitude
:param point_b: A LatLon object defined in pygeodesy package. First parameter is Latitude and second is Longitude
:return: The distance between point_a and point_b as a float in nautical miles
>>> storm_distance_bearing(pg.LatLon('0.0N', '90.0W'), pg.LatLon('0.0N', '90.0W'))
[0, -1]
>>> storm_distance_bearing(pg.LatLon('15.0N', '59.0W'), pg.LatLon('16.0N', '60.6W'))
[110.28249410147036, 302.58817298995035]
"""
# If two points are the same, return 0 as distance. Otherwise throw exception
if point_a == point_b:
return [0, -1]
# If two points are different, calculate distance and final bearing
else:
dist_bear = point_a.distanceTo3(point_b)
return [dist_bear[0] / 1852.0, dist_bear[1]]
def loc_at_conflict(self):
# ownship : lon [deg] lat[deg] alt[feet] trk[deg] gs[knots] vs [ft/min]
# traffic : lon [deg] lat[deg] alt[feet] trk[deg] gs[knots] vs [ft/min]
if not self.cd.conflict:
# no conflict
return False
elif not self.gxy:
return False # This method only works with geodesic coordinates
else:
entry_alt_o = round(self.alt_o +
self.vs_o / 60 * self.t_in) # [ft]
exit_alt_o = round(self.alt_o +
self.vs_o / 60 * self.t_out) # [ft]
entry_alt_i = round(self.alt_i +
self.vs_i / 60 * self.t_in) # [ft]
exit_alt_i = round(self.alt_i +
self.vs_i / 60 * self.t_out) # [ft]
dist2entry_o = knots2msec(self.gs_o) * self.t_in # [m]
dist2exit_o = knots2msec(self.gs_o) * self.t_out # [m]
dist2entry_i = knots2msec(self.gs_i) * self.t_in # [m]
dist2exit_i = knots2msec(self.gs_i) * self.t_out # [m]
current_loc_o = LatLon(self.x_o, self.y_o)
current_loc_i = LatLon(self.x_o, self.y_i)
entry_o = current_loc_o.destination(dist2entry_o, self.trk_o)
exit_o = current_loc_o.destination(dist2exit_o, self.trk_o)
entry_i = current_loc_i.destination(dist2entry_i, self.trk_i)
exit_i = current_loc_i.destination(dist2exit_i, self.trk_i)
self.loc_at_entry_o = [entry_o.lat, entry_o.lon, entry_alt_o]
self.loc_at_exit_o = [exit_o.lat, exit_o.lon, exit_alt_o]
self.loc_at_entry_i = [entry_i.lat, entry_i.lon, entry_alt_i]
self.loc_at_exit_i = [exit_i.lat, exit_i.lon, exit_alt_i]
def default_start_location(self):
query = EntitiesQuery()
query.set_limit(1)
query.set_included_discriminators(["Place"])
query.add_condition(FieldNamed("name").eq(self._name))
candidates = self._db.get_entities(query)
if not candidates:
log.warn(
"Area %s does not have a Place entity, falling back to the first entity in the database.",
self._name)
query = EntitiesQuery()
query.set_limit(1)
candidates = self._db.get_entities(query)
entity = candidates[0]
geom = wkb.loads(entity.geometry)
if not isinstance(geom, Point):
geom = geom.representative_point()
lon = geom.x
lat = geom.y
return LatLon(lat, lon)
def detection_map(map_det_obj_str: str, build_map: int, id: int):
"""
Detection map function for a single image.
:param map_det_obj_str: String representing a MapDetectionObj
:param build_map: Determines if a map is created. Pass 1 if a map should be created, 0 otherwise.
:return: Dict containing the names and the GPS-coordinates of all detected object
"""
map_det_obj = DetectionMapObject.from_string(map_det_obj_str)
image, lat, lon, start_bearing, scaling_factor_z, scaling_factor_x = map_det_obj.unpack(
)
start_point = LatLon(lat, lon)
d = DetectedObjectGPS()
gps = d.run(start_point, start_bearing, image, scaling_factor_z,
scaling_factor_x, id)
x, z = d.gps_to_global_xz(start_point, start_bearing, gps)
d.plot_data(x, z, id)
if build_map:
d.create_map(lat, lon, id)
return d.classified_map_detections
def _on_map_ready(self):
self.setWindowTitle(f"{self._selected_map_name} - Feel the streets")
if is_frozen_area(self._selected_map):
original_id = self._selected_map - FROZEN_AREA_OSM_ID_OFFSET
area_name = get_area_names_cache()[original_id]
else:
area_name = self._selected_map_name
map.set_call_args(self._selected_map, area_name)
menu_service.set_call_args(self)
self._app_controller = ApplicationController(self)
person = Person(map=map(), position=LatLon(0, 0))
self._person_controller = InteractivePersonController(person, self)
self._interesting_entities_controller = InterestingEntitiesController(
person)
self._sound_controller = SoundController(person)
self._announcements_controller = AnnouncementsController(person)
self._last_location_controller = LastLocationController(person)
self._restriction_controller = MovementRestrictionController(person)
self._speech_controller = SpeechController()
self._adjustment_controller = PositionAdjustmentController(person)
if not self._last_location_controller.restored_position:
person.move_to(map().default_start_location, force=True)
self.raise_()
menu_service().ensure_key_capturer_focus()
def last_location(self):
loc = self._last_location_entity
if loc:
return LatLon(loc.latitude, loc.longitude), loc.direction
else:
return None
def generateCoOrdinates(latitude, longitude, degrees, secret_key_google,
main_latitude, main_longitude, key):
"""
:param latitude: source latitude
:param longitude: source longitude
:param degrees: directions where co-ordinates should be taken for
:param secret_key_google: secret key of google api
:param main_latitude: main latitude of souce
:param main_longitude: main longitude of source
:param key: Google/Bing key
:return: Dictionary of source and destination coordinates with distance
"""
i = 0
drops = 0
m = 0
hashset = set()
priority_queue = [(0, main_latitude, main_longitude)]
list_queue = [(0, 0, 0, 0)]
distance = 0
geo_distance = {}
maxvalue = -99999.99
final_input = []
count = 0
source_final = []
dest_final = []
dist_final = []
while i < 100000 and len(list_queue) > 0:
if i == 0:
list_queue.pop(0)
else:
maxvalue, distance, latitude, longitude = list_queue.pop(0)
latlong = str(latitude) + "," + str(longitude)
j = 0
for k in range(0, 4):
p = LatLon(latitude, longitude)
latitude1 = np.round(p.destination(10, degrees[j]).lat, 6)
longitude1 = np.round(p.destination(10, degrees[j]).lon, 6)
if maxvalue < 1000:
count += 1
total_distance = checkForPathGoogle(latitude1, longitude1,
latitude, longitude, key)
if (latitude, longitude) in geo_distance:
geo_distance[(latitude, longitude)].update({
(latitude1, longitude1):
total_distance
})
else:
geo_distance[(latitude, longitude)] = {
(latitude1, longitude1): total_distance
}
if (latitude1, longitude1) not in hashset:
maxvalue = haversine(main_latitude, main_longitude,
latitude1, longitude1)
hashset.add((latitude1, longitude1))
list_queue.append(
(maxvalue, total_distance, latitude1, longitude1))
j = (j + 1) % 4
else:
drops += 1
break
i += 1
return geo_distance
def test1():
p = LatLon(-37.95103, 144.42487)
d = p.destination(54972.271, 306.86816)
print(d.lon, d.lat)
def update(self, val):
self.iter+= 1
if self.avgPos[0] == None:
self.avgPos = [ val['lat'],val['lon'] ]
self.lat = val['lat']
self.lon = val['lon']
self.avgPos[0] = (self.avgPos[0]*self.avgReadings)+(val['lat']*(1.0-self.avgReadings))
self.avgPos[1] = (self.avgPos[1]*self.avgReadings)+(val['lon']*(1.0-self.avgReadings))
doIt = True
tSinceLast = (self.th.getTimestamp(True)-self.updateTime)/1000000.0
if tSinceLast < 0.5:
doIt = False
else:
#print("-----------------")
#print("time since last",tSinceLast)
pavg = LatLon( self.avgPos[0], self.avgPos[1] )
pNew = LatLon( val['lat'], val['lon'])
dis = pavg.distanceTo(pNew)
spe = (( dis/1000.00 )/tSinceLast)*60.0*60.0
self.sog = (spe*0.539957) # to nm
self.cog = pavg.bearingTo(pNew)
#print("distance is ",dis)
if spe > 100.00:
doIt = False
print( "gps data Dump to fast ! ",
"Speed is ",spe," km/h"
)
if doIt:
self.lat = val['lat']
self.guiObjs['lat'].text = "%s"%self.lat
self.lon = val['lon']
self.guiObjs['lon'].text = "%s"%self.lon
self.gpssog = val['speed']
self.guiObjs['sog'].text = "%s / %s"%(round(self.gpssog,2), round(self.sog,2))
self.guiObjs['lSRacSog'].text = "%s" % round(self.sog,1)
if self.maxSog < self.sog:
self.maxSog = self.sog
self.guiObjs['lSRacSogMax'].text = "max: %s" % round(self.maxSog,2)
self.avgSog = (self.avgSog*0.998)+(self.sog*0.002)
val['avgSog'] = self.avgSog
self.guiObjs['lSRacSogAvg'].text = "avg: %s" % round(self.avgSog,2)
self.gpscog = val['bearing']
self.guiObjs['cog'].text = "%s / %s"%(round(self.gpscog,1), round(self.cog,1))
self.avgCog = (self.avgCog*0.998)+(self.gpssog*0.002)
val['avgCog'] = self.avgCog
self.accur = val['accuracy']
self.guiObjs['accur'].text = "%s"%round(self.accur,0)
self.updateTime = self.th.getTimestamp(True)
# nmea
latRaw = dms.latDMS( self.lat,form=dms.F_DM,prec=6).replace('°','').replace('′','')
latDM = latRaw[:-1]
latNS = latRaw[-1]
lonRaw = dms.lonDMS( self.lon,form=dms.F_DM,prec=6).replace('°','').replace('′','')
lonDM = lonRaw[:-1]
lonEW = lonRaw[-1]
msg = ("$YKRMC,,A,%s,%s,%s,%s,%s,%s,,,,A"%(latDM,latNS,lonDM,lonEW,round(self.sog,2),round(self.cog,2)))
self.gui.sf.sendToAll(msg)
if self.gui.isReady:
# callbacks
for o in self.callBacksForUpdate:
o.update('gps', val)
# json
jMsg = str({
"type": "gps",
"data": val
})
self.gui.sf.sendToAll( jMsg )
def calculate_bistatic_target_parameters(radar_lat, radar_lon, radar_ele,
radar_bearing, ioo_lat, ioo_lon,
ioo_ele, target_lat, target_lon,
target_ele, target_speed, target_dir,
wavelength):
"""
Description:
------------
This function calculates the bistatic range, bistatic Doppler frequency,
and bearing angle of the observed target using the positions and the
velocities of the target and the location of the radar and the IoO.
Dependencies:
-------------
- PyGeodesy
Parameters:
-----------
:param radar_lat: Radar latitude coordinate [deg]
:param radar_lon: Radar longitude coordinate [deg]
:param radar_ele: Radar elevation [m]
:param radar_bearing: Boresight direction of the surveillance antenna of the radar.
Interpreted as clockwise from the north pole [deg]
:param ioo_lat: Transmitter latitude coordinate [deg]
:param ioo_lon: Transmitter longitude coordinate [deg]
:param ioo_ele: Transmitter elevation [m]. For broadcast towers this is typically
defined as the sum of the ASL and AGL hegihts.
ASL: Above Seal Level
AGL: Above Ground Level
:param target_lat: Target latitude coordinate [deg]
:param target_lon: Target longitude coordinate [deg]
:param target_ele: Target altitude [m]
:param target_speed: Target ground speed [meter per second]
:param target_dir: Target moving direction [deg]
Interpreted as clockwise from the north pole [deg]
:param wavelength: Wavelength of the used IoO [m]
:type radar_lat: float [-90 .. 90]
:type radar_lon: float [-90 .. 90]
:type radar_ele: float
:type radar_bearing: float [0 .. 180]
:type ioo_lat: float [-90 .. 90]
:type ioo_lon: float [-90 .. 90]
:type ioo_ele: float
:type target_lat: float [-90 .. 90]
:type target_lon: float [-90 .. 90]
:type target_ele: float
:type target_speed: flat
:type target_dir: float [0 .. 180]
:type wavelength: float
Return values:
--------------
:return calculated target parameters:
(target bistatic range [m],
target Doppler frequency [Hz]],
target azimuth bearing [deg])
:rtype calculated target parameters: python list with 3 elements
[float, float, float]
"""
ecef_converter = ecef.EcefYou(Datums.Sphere)
# Convert geodetic radar position to ECEF coordinates
radar_ecef = ecef_converter.forward(radar_lat, radar_lon, radar_ele)
radar_ecef = np.array([radar_ecef[0], radar_ecef[1], radar_ecef[2]])
# Convert geodetic IoO position to ECEF coordinates
ioo_ecef = ecef_converter.forward(ioo_lat, ioo_lon, ioo_ele)
ioo_ecef = np.array([ioo_ecef[0], ioo_ecef[1], ioo_ecef[2]])
# Baseline distance
L = np.sqrt(np.sum(np.abs(radar_ecef - ioo_ecef)**2))
# Convert geodetic (lat, lon, elevation) target position to ECEF coordinates
target_ecef = ecef_converter.forward(target_lat, target_lon, target_ele)
target_ecef = np.array([target_ecef[0], target_ecef[1], target_ecef[2]])
# Target to IoO distance
Rt = np.sqrt(np.sum(np.abs(target_ecef - ioo_ecef)**2))
# Target to radar distance
Rr = np.sqrt(np.sum(np.abs(target_ecef - radar_ecef)**2))
# Bistatic distance
Rb = Rt + Rr - L
# Generate speed vector
target_latlon = LatLon(target_lat, target_lon) # Target initial coordinate
speed_vector_latlon = target_latlon.destination(
1, target_dir) # Target coordinate 1m away in the current direction
speed_vector_ecef = ecef_converter.forward(speed_vector_latlon.lat,
speed_vector_latlon.lon,
target_ele)
speed_vector_ecef = np.array(
[speed_vector_ecef[0], speed_vector_ecef[1], speed_vector_ecef[2]])
speed_vector = speed_vector_ecef - target_ecef # Create vector in Cartesian space
speed_vector /= np.sqrt(np.sum(np.abs(speed_vector)**2)) # Normalize
# Generate target to IoO vector
target_to_ioo_vector = (ioo_ecef - target_ecef) / Rt
# Generate target to radar vector
target_to_radar_vector = (radar_ecef - target_ecef) / Rr
# Calculate target Doppler
fD = 1*(target_speed / wavelength) * \
( (np.sum(speed_vector*target_to_ioo_vector)) + (np.sum(speed_vector*target_to_radar_vector)))
# Caculate target azimuth direction
# Formula is originated from: https://www.movable-type.co.uk/scripts/latlong.html
lat1 = np.deg2rad(radar_lat)
lon1 = np.deg2rad(radar_lon)
lat2 = np.deg2rad(target_lat)
lon2 = np.deg2rad(target_lon)
target_doa = np.arctan2(
np.cos(lat1) * np.sin(lat2) -
np.sin(lat1) * np.cos(lat2) * np.cos(lon2 - lon1),
np.sin(lon2 - lon1) * np.cos(lat2))
theta = 90 - np.rad2deg(target_doa) - radar_bearing + 90
return (Rb, fD, theta)