def __publish_thread(self):
"""
MQTT publish closest observation every second, more often if the plane is closer
"""
while True:
if not self.__tracking_icao24:
time.sleep(1)
else:
if not self.__tracking_icao24 in self.__observations:
self.__tracking_icao24 is None
continue
cur = self.__observations[self.__tracking_icao24]
if cur is None:
continue
(lat, lon) = utils.calc_travel(cur.getLat(), cur.getLon(), cur.getLoggedDate(), cur.getGroundSpeed(), cur.getHeading())
distance = utils.coordinate_distance(self.__latitude, self.__longitude, lat, lon)
# Round off to nearest 100 meters
distance = round(distance/100) * 100
bearing = utils.bearing(self.__latitude, self.__longitude, lat, lon)
# @todo: update altitude
# altitude = sbs1["altitude"]
retain = False
self.__mqtt_bridge.publish(self.__prox_topic, cur.json(bearing, distance), 0, retain)
logging.info("%s at %5d brg %3d alt %5d trk %3d spd %3d %s" % (cur.getIcao24(), distance, bearing, cur.getAltitude(), cur.getHeading(), cur.getGroundSpeed(), cur.getType()))
if distance < 3000:
time.sleep(0.25)
elif distance < 6000:
time.sleep(0.5)
else:
time.sleep(1)
def test_bearing():
"""Unit test for bearing()."""
# Example from: https://www.igismap.com/formula-to-find-bearing-or-heading-angle-between-two-points-latitude-longitude/
lat1, long1 = 39.099912, -94.581213
lat2, long2 = 38.627089, -90.200203
expected_bearing = 96.51262423499941
assert bearing(lat1, long1, lat2, long2) == expected_bearing
def bearings(self):
b = []
p_from = self.path[0]
for p_to in self.path[1:]:
b.append(int(bearing(p_from, p_to)))
p_from = p_to
return b
def create_path(a, b, contour, distancia, dir=0):
conf_1 = -90
conf_2 = -1
conf_3 = "left"
conf_4 = 0
if dir == 1:
conf_3 = "right"
path = []
contorno_points = []
for c in contour:
c = utils.to_utm(c)
utm_pos = Point(c.x, c.y)
contorno_points.append(utm_pos)
a2 = utils.to_utm(a)
a_utm = Point(a2.x, a2.y)
b2 = utils.to_utm(b)
b_utm = Point(b2.x, b2.y)
ab_course = utils.bearing(a_utm, b_utm)
a_utm = utils.offset(a2, ab_course - 90, distancia * 80)
b_utm = utils.offset(b2, ab_course - 90, distancia * 80)
a2, b2 = utils.extend(a_utm, b_utm)
AB = LineString([a2, b2])
contorno = Polygon(contorno_points)
eroded = contorno.buffer(-distancia, resolution=16, join_style=1)
line = AB.intersection(eroded)
for x in range(1, 200):
ab_1 = AB.parallel_offset(x * distancia, conf_3)
line = ab_1.intersection(eroded)
if (line.geom_type == "LineString"):
if (len(line.coords) > 1):
p1 = 1
p2 = conf_2
if x % 2 == 0:
p1 = 0
p2 = -conf_2
centro = utils.offset(utils.toCoord(line.coords[p1]),
ab_course + conf_1, distancia / 2)
radius = distancia / 2
start_angle, end_angle = 90 - ab_course - 90, 90 - ab_course + 90 # In degrees
if x % 2 == 0:
start_angle, end_angle = 90 - ab_course + 90, 90 - ab_course - 90
numsegments = 200
theta = np.radians(
np.linspace(start_angle, end_angle, numsegments))
x = centro.x + (radius * np.cos(theta)) * p2
y = centro.y + (radius * np.sin(theta)) * p2
arc = LineString(np.column_stack([x, y]))
for c in arc.coords:
path.append(Point(c))
final = LineString(path)
path2 = []
for x in range(0, int(final.length / 2)):
p = final.interpolate(x * 2)
path2.append(p)
return path2
def move(LatLon):
global old_pos, init, old_bearing, mesh, bearings, tractor
utm_pos = Coordinate(x=LatLon.x, y=LatLon.y)
if init is False:
tractor.m_p_s = 2
tractor.run()
init = True
old_pos = utm_pos
scene.camera.pos = vec(utm_pos.x, 4, utm_pos.y - 50)
scene.camera.axis = vec(0, -4, -50)
obj = []
for x in range(-50, 50):
for y in range(-50, 50):
a = Coordinate(x=utm_pos.x + x * 500,
y=utm_pos.y + y * 500 + 500)
b = Coordinate(x=utm_pos.x + x * 500 + 500,
y=utm_pos.y + y * 500 + 500)
c = Coordinate(x=utm_pos.x + x * 500 + 500,
y=utm_pos.y + y * 500)
d = Coordinate(x=utm_pos.x + x * 500, y=utm_pos.y + y * 500)
Q = quad(
canvas=None,
v0=vertex(pos=vec(a.x, -1, a.y)),
v1=vertex(pos=vec(b.x, -1, b.y)),
v2=vertex(pos=vec(c.x, -1, c.y)),
v3=vertex(pos=vec(d.x, -1, d.y)),
)
obj.append(Q)
compound(obj, canvas=scene, color=vec(0, .60, 0))
else:
curso = utils.bearing(utm_pos, old_pos)
a = utils.offset(utm_pos, curso - 90, distance / 2)
b = utils.offset(utm_pos, curso + 90, distance / 2)
c = utils.offset(old_pos, old_bearing + 90, distance / 2)
d = utils.offset(old_pos, old_bearing - 90, distance / 2)
Q = quad(
v0=vertex(pos=vec(a.x, .5, a.y), color=color.yellow),
v1=vertex(pos=vec(b.x, .5, b.y), color=color.yellow),
v2=vertex(pos=vec(c.x, .5, c.y), color=color.yellow),
v3=vertex(pos=vec(d.x, .5, d.y), color=color.yellow),
)
qobj.append(Q)
left.append(vec(a.x, .5, a.y))
right.append(vec(b.x, .5, b.y))
old_bearing = curso
old_pos = utm_pos
bearings.pop()
bearings.insert(0, curso)
curso = sum(bearings) / len(bearings)
camera = utils.offset(utm_pos, curso, 300)
x, z = utils.vector(curso + 180)
#scene.camera.pos=vec(camera.x, 100, camera.y)
#scene.camera.axis=200*vec(x, -0.3 , z)
tractor_3d.pos = tractor.get_vec(1.5)
def update(self, sbs1msg):
oldData = dict(self.__dict__)
#self.__loggedDate = datetime.utcnow()
if sbs1msg["icao24"]:
self.__icao24 = sbs1msg["icao24"]
if sbs1msg["callsign"] and self.__callsign != sbs1msg["callsign"]:
self.__callsign = sbs1msg["callsign"].rstrip()
if sbs1msg["altitude"]:
self.__altitude = sbs1msg["altitude"]
self.__altitudeTime = datetime.utcnow()
if sbs1msg["groundSpeed"]:
self.__groundSpeed = sbs1msg["groundSpeed"]
if sbs1msg["track"]:
self.__track = sbs1msg["track"]
if sbs1msg["lat"]:
self.__lat = sbs1msg["lat"]
self.__latLonTime = datetime.utcnow()
if sbs1msg["lon"]:
self.__lon = sbs1msg["lon"]
self.__latLonTime = datetime.utcnow()
if sbs1msg["verticalRate"]:
self.__verticalRate = sbs1msg["verticalRate"]
if not self.__verticalRate:
self.__verticalRate = 0
if sbs1msg["loggedDate"]:
self.__loggedDate = datetime.strptime(sbs1msg["loggedDate"],
'%Y-%m-%d %H:%M:%S.%f')
if sbs1msg["registration"]:
self.__registration = sbs1msg["registration"]
if sbs1msg["manufacturer"]:
self.__manufacturer = sbs1msg["manufacturer"]
if sbs1msg["model"]:
self.__model = sbs1msg["model"]
if sbs1msg["operator"]:
self.__operator = sbs1msg["operator"]
if sbs1msg["type"]:
self.__type = sbs1msg["type"]
# Calculates the distance from the cameras location to the airplane. The output is in METERS!
distance = utils.coordinate_distance(camera_latitude, camera_longitude,
self.__lat, self.__lon)
#Not sure we want to... commented out for now -> Round off to nearest 100 meters
self.__distance = distance = distance #round(distance/100) * 100
self.__bearing = utils.bearing(camera_latitude, camera_longitude,
self.__lat, self.__lon)
self.__elevation = utils.elevation(
distance, self.__altitude,
camera_altitude) # Distance and Altitude are both in meters
# Check if observation was updated
newData = dict(self.__dict__)
#del oldData["_Observation__loggedDate"]
#del newData["_Observation__loggedDate"]
d = DictDiffer(oldData, newData)
self.__updated = len(d.changed()) > 0
def cross_err(pos, nav, path):
print(nav)
print(pos.x, pos.y)
position = utils.offset(pos, nav, 4)
print(position.x, position.y)
punto = nearest_points(position, path)[1]
curso = utils.bearing(position, punto)
lado = utils.get_diff(nav, curso)
distancia = punto.distance(position)
distancia = math.copysign(distancia, lado)
return distancia
def __publish_thread(self):
"""
MQTT publish closest observation every second, more often if the plane is closer
"""
timeHeartbeat = 0
while True:
if timeHeartbeat < time.mktime(time.gmtime()):
timeHeartbeat = time.mktime(time.gmtime()) + 10
self.__client.publish("skyscan/heartbeat",
"skyscan-tracker-" + ID + " Heartbeat",
0, False)
if not self.__tracking_icao24:
time.sleep(1)
else:
if not self.__tracking_icao24 in self.__observations:
self.__tracking_icao24 is None
continue
cur = self.__observations[self.__tracking_icao24]
if cur is None:
continue
(lat, lon) = utils.calc_travel(cur.getLat(), cur.getLon(),
cur.getLoggedDate(),
cur.getGroundSpeed(),
cur.getTrack(), camera_lead)
distance = utils.coordinate_distance(camera_latitude,
camera_longitude, lat,
lon)
# Round off to nearest 100 meters
#distance = round(distance/100) * 100
bearing = utils.bearing(camera_latitude, camera_longitude, lat,
lon)
elevation = utils.elevation(
distance, cur.getAltitude(), camera_altitude
) # we need to convert to feet because the altitude is in feet
retain = False
self.__client.publish(self.__tracking_topic,
cur.json(bearing, distance, elevation),
0, retain)
logging.info(
"%s at %5d brg %3d alt %5d trk %3d spd %3d %s" %
(cur.getIcao24(), distance, bearing, cur.getAltitude(),
cur.getTrack(), cur.getGroundSpeed(), cur.getType()))
if distance < 3000:
time.sleep(0.25)
elif distance < 6000:
time.sleep(0.5)
else:
time.sleep(1)
def bearing_to_next(self):
b = 0.0
c = 0.0
cur = self
#FIXME: weighted average based on distances
while True:
try:
next = cur.get_next_in_order()
b += bearing(cur.mark.location.tuple, next.mark.location.tuple)
c += 1
except CourseMark.DoesNotExist:
break
if not next.is_waypoint:
break
cur = next
return b/c if c else None
def bearing_from_previous(self):
b = 0.0
c = 0.0
cur = self
#FIXME: weighted average based on distances
while True:
try:
prev = cur.get_previous_in_order()
b += bearing(prev.mark.location.tuple, cur.mark.location.tuple)
c += 1
except CourseMark.DoesNotExist:
break
if not prev.is_waypoint:
break
cur = prev
return b/c if c else None
xx, yy = transform(wgs84, UTM, c.y, c.x)
limit_line.append(vec(xx, .5, yy))
xx, yy = transform(wgs84, UTM, puntoA.y, puntoA.x)
puntoA_UTM = Coordinate(x=xx, y=yy)
qtemp = box(color=color.red, size=vec(10, 10, 10), pos=vec(xx, .5, yy))
xx, yy = transform(wgs84, UTM, puntoB.y, puntoB.x)
puntoB_UTM = Coordinate(x=xx, y=yy)
qtemp = box(color=color.red, size=vec(10, 10, 10), pos=vec(xx, .5, yy))
path = nav.create_path(puntoA, puntoB, coords2, 19, dir=dir2)
for c in path:
ab.append(vec(c.x, .5, c.y))
tractor.location = path[0]
print(
"AB COURSE",
utils.bearing(puntoA_UTM, puntoB_UTM),
'dir',
dir2,
)
def b_action(b):
global left, right, qtemp, qobj
qtemp.visible = False
left.visible = False
right.visible = False
del left, right
left = curve(color=color.orange, radius=0.3, retain=500)
right = curve(color=color.orange, radius=0.3, retain=500)
tractor.location = Coordinate(x=(path[0].x - 50), y=(path[0].y - 20))
for obj in qobj:
mode = gps.net.get_mode(mode)
while mode == "APAGAR": #APAGAR
sleep(1)
mode = gps.net.get_mode(mode)
while mode == "MANUAL":
step, direccion = gps.net.get_test()
print("ORDENES DE GIRO:", step, direccion)
control.crear_giro(step, direccion)
mode = gps.net.get_mode(mode)
sleep(1)
if mode == "TARGET":
target = utils.to_utm(gps.net.get_target())
tractor = navigator.Tractor()
while mode == "TARGET":
bearing = gps.nav
target_course = utils.bearing(utils.to_utm(gps.pos()), target)
dif = utils.get_diff(bearing, target_course)
print("bearing", bearing, "target", target_course, "dif", dif)
calc = tractor.doblar(dif, 0)
print(calc)
control.crear_giro(calc)
mode = gps.net.get_mode(mode)
sleep(.15)
if mode == "GRABAR LIMITE":
limite = []
gps.net.clear_mission()
while mode == "GRABAR LIMITE":
try:
c = gps.pos()
limite.append(c)
mode = gps.net.get_mode(mode)
