Ejemplo n.º 1
0
    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)
Ejemplo n.º 2
0
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
Ejemplo n.º 3
0
 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
Ejemplo n.º 4
0
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
Ejemplo n.º 5
0
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)
Ejemplo n.º 6
0
    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
Ejemplo n.º 7
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
Ejemplo n.º 8
0
    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)
Ejemplo n.º 9
0
 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
Ejemplo n.º 10
0
    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
Ejemplo n.º 11
0
    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:
Ejemplo n.º 12
0
     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)