Example #1
0
    def post_telemetry(self, location, heading):
        """
        Post the drone's telemetry information.

        :param location:    The location to post.
        :type location:     Location
        :param heading:     The UAV's heading.
        :type heading:      Float
        """
        missing_data = True

        while missing_data:
            try:
                telem_upload_data = Telemetry(
                    location.get_lat(), location.get_lon(),
                    location.get_alt() + GCSSettings.MSL_ALT, heading)
                missing_data = False
            except ConnectionError:
                sleep(GCSSettings.INTEROP_DISCONNECT_RETRY_RATE)

                try:
                    self.client = Client(GCSSettings.INTEROP_URL,
                                         GCSSettings.INTEROP_USERNAME,
                                         GCSSettings.INTEROP_PASSWORD)
                except:
                    print("Failed to connect to Interop., retrying...")

        self.client.post_telemetry(telem_upload_data)
Example #2
0
def rate_publisher(device, client):
    pub = rospy.Publisher('/connections/judges/hz', Float32, queue_size=10)
    rospy.init_node('interop', anonymous=True)
    period = 4
    mav = mavutil.mavlink_connection(device, autoreconnect=True)
    sent_since_print = 0
    last_print = rospy.get_time()
    while not rospy.is_shutdown():
        msg = mav.recv_match(type='GLOBAL_POSITION_INT',
                             blocking=True,
                             timeout=10.0)
        if msg is None:
            rospy.loginfo('Did not receive mavlink message')
            sys.exit(1)
        telemetry = Telemetry(latitude=mavlin_latlon(msg.lat),
                              longitude=mavlink_latlon(msg.lon),
                              altitude_msl=mavlink_alt(msg.alt),
                              uas_heading=mavlink_heading(msg.hdg))
        try:
            client.post_telemetry(telemetry)
        except:
            rospy.logerr('Telemetry Post Error')
            sys.exit(1)
        sent_since_print += 1
        now = rospy.get_time()
        since_print = now - last_print
        if since_print > period:
            telem_rate = sent_since_print / since_print
            pub.publish(telem_rate)
            sent_since_print = 0
            last_print = now
Example #3
0
def proxy_mavlink(device, client):
    """Receives packets over the device and forwards telemetry via the client.

    Args:
        device: A pymavlink device name to forward.
        client: Interop Client with which to send telemetry packets.
    """

    # Create the MAVLink connection.
    mav = mavutil.mavlink_connection(device, autoreconnect=True)

    # Track rates.
    sent_since_print = 0
    last_print = time.time()
    client = interop.Client(url='http://10.42.0.1:8000',
                            username='******',
                            password='******')

    # Continuously forward packets.
    while True:
        # Get packet.
        msg = mav.recv_match(type='GLOBAL_POSITION_INT',
                             blocking=True,
                             timeout=10.0)
        if msg is None:
            logger.critical(
                'Did not receive MAVLink packet for over 10 seconds.')
            sys.exit(-1)
        # Convert to telemetry.
        telemetry = Telemetry(latitude=mavlink_latlon(msg.lat),
                              longitude=mavlink_latlon(msg.lon),
                              altitude_msl=mavlink_alt(msg.alt),
                              uas_heading=mavlink_heading(msg.hdg))
        uas_head = mavlink_heading(msg.hdg)
        lat = mavlink_latlon(msg.lat),
        longi = mavlink_latlon(msg.lon),
        alt = mavlink_alt(msg.alt)
        #print uas_head
        #print lat
        #print longi
        #print alt
        client.post_telemetry(telemetry)
        # Forward telemetry.
        #try:
        #client.post_telemetry(telemetry)
        #except:
        #logger.exception('Failed to post telemetry to interop.')
        #sys.exit(-1)
        # Track telemetry rates.
        print(uas_head, lat, longi, alt)
        sent_since_print += 1
        now = time.time()
        since_print = now - last_print
        if since_print > PRINT_PERIOD:
            logger.info('Telemetry rate: %f', sent_since_print / since_print)
            sent_since_print = 0
            last_print = now
Example #4
0
 def mavlink_packet(self, m):
     '''handle a mavlink packet'''
     if m.get_type() == 'GLOBAL_POSITION_INT':
         telemetry = Telemetry(
             float(m.lat) / 10000000,
             float(m.lon) / 10000000,
             float(m.alt) / 1000,
             float(m.hdg) / 100)
         self.client.post_telemetry(telemetry)
Example #5
0
    def telemetry(self, lat, lon, alt, heading):
        t = Telemetry(latitude=lat,
                      longitude=lon,
                      altitude_msl=alt,
                      uas_heading=heading)
        self.client.post_telemetry(t)

        new_time = time()
        print(1 / (new_time - self.last_telemetry))
        self.last_telemetry = new_time

        return True
Example #6
0
    def post_telemetry(self, location, heading):
        """
        Post the drone's telemetry information

        :param location: The location to post
        :type location: Location
        :param heading: The UAV's heading
        :type heading: Float
        """
        telem_upload_data = Telemetry(location.get_lat(), location.get_lon(), location.get_alt() + GCSSettings.MSL_ALT, heading)

        self.client.post_telemetry(telem_upload_data)
Example #7
0
def rate_publisher(client):
    pub = rospy.Publisher('/connections/judges/hz', Float32, queue_size=10)
    rospy.init_node('interop', anonymous=True)
    period = 4
    sent_since_print = 0
    last_print = rospy.get_time()
    while not rospy.is_shutdown():
        telemetry = Telemetry(0, 0, 0, 0)
        telemetry_resp = client.post_telemetry(telemetry)
        sent_since_print += 1
        now = rospy.get_time()
        since_print = now - last_print
        if since_print > period:
            telem_rate = sent_since_print / since_print
            pub.publish(telem_rate)
            sent_since_print = 0
            last_print = now
Example #8
0
def probe(args, client):
    while True:
        start_time = datetime.datetime.now()

        telemetry = Telemetry(0, 0, 0, 0)
        telemetry_resp = client.post_telemetry(telemetry)
        obstacle_resp = client.get_obstacles()

        end_time = datetime.datetime.now()
        elapsed_time = (end_time - start_time).total_seconds()
        logger.info('Executed interop. Total latency: %f', elapsed_time)

        delay_time = args.interop_time - elapsed_time
        if delay_time > 0:
            try:
                time.sleep(delay_time)
            except KeyboardInterrupt:
                sys.exit(0)
Example #9
0
    def get_uas_telemetry(self, new_time):
        delta_time = (new_time - self.time).total_seconds()
        self.time = new_time
        move_distance = self.speed * delta_time

        delta_altitude = delta_time * max_climb_rate
        delta_heading = delta_time * max_turn_rate

        while move_distance > 0:
            vec = self.pos.vector(self.path[self.waypoint])

            # Way-point Switching
            if vec.distance < switch_threshold:
                if self.waypoint + 1 == len(self.path):
                    break
                else:
                    self.waypoint += 1
                    continue

            # Climb as needed
            vec.altitude = max(vec.altitude, -delta_altitude)
            vec.altitude = min(vec.altitude, delta_altitude)
            delta_altitude -= abs(vec.altitude)

            # Turn as needed
            vec.vector.heading, delta_heading = calc_new_heading(
                self.pos.heading, vec.heading, delta_heading)

            vec.distance = move_distance if vec.distance > move_distance else vec.distance
            move_distance -= vec.distance

            # Apply motion
            self.pos.move(vec)

        return Telemetry(self.pos.latitude, self.pos.longitude,
                         self.pos.altitude, self.pos.heading)
Example #10
0
 def get_uas_telemetry(self, cur_time):
     """Overrides base method."""
     return Telemetry(0, 0, 0, 0)
Example #11
0
def proxy_mavlink(device, client):
    """Receives packets over the device and forwards telemetry via the client.

    Args:
        device: A pymavlink device name to forward.
        client: Interop Client with which to send telemetry packets.
    """
    # Create the MAVLink connection.
    print(device)
    mav = mavutil.mavlink_connection(device, baud=57600, autoreconnect=True)

    # Track rates.
    sent_since_print = 0
    last_print = time.time()

    # Continuously forward packets.
    while True:
        # Get packets
        start = time.time()
        alt_and_hdg = mav.recv_match(type= 'VFR_HUD',#'GLOBAL_POSITION_INT',
                             blocking=True,
                             timeout=10.0)
        #msg = mav.recv_msg()
        if alt_and_hdg is None:
            logger.critical(
                'Did not receive MAVLink packet for over 10 seconds.')
            sys.exit(-1)
       	#received_VFR = time.time()
        long_and_lat = mav.recv_match(type= 'GPS_RAW_INT',#'GLOBAL_POSITION_INT',
                             blocking=True,
                             timeout=10.0)

        if long_and_lat is None:
            logger.critical(
                'Did not receive MAVLink packet for over 10 seconds.')
            sys.exit(-1)
        #received_GPS = time.time()
        # Convert to telemetry.
        
        telemetry = Telemetry(latitude=mavlink_latlon(long_and_lat.lat),
                              longitude=mavlink_latlon(long_and_lat.lon),
                              altitude_msl=mavlink_alt(alt_and_hdg.alt*1000),#mavlink_alt(msg.alt),
                              uas_heading=alt_and_hdg.heading)#mavlink_heading(alt_and_hdg.heading))
        
        print(telemetry)
        # Forward telemetry.
        try:
            client.post_telemetry(telemetry)
        except:
            logger.exception('Failed to post telemetry to interop.')
            sys.exit(-1)

        #posted_telemetry = time.time()
        # Track telemetry rates.
        sent_since_print += 1
        now = time.time()
        since_print = now - last_print
        if since_print > PRINT_PERIOD:
            logger.info('Telemetry rate: %f', sent_since_print / since_print)
            sent_since_print = 0
            last_print = now
Example #12
0
def testTelemtry(client):
    rand = generate_random_data(38.142544, -76.434088, 10000)
    for w in rand:
        telemetry = Telemetry(w['latitude'], w['longitude'], 200, 90)
        client.post_telemetry(telemetry)
        time.sleep(0.5)
Example #13
0
def testWaypoints(client, mission):
    for w in mission.mission_waypoints:
        telemetry = Telemetry(w.latitude, w.longitude, w.altitude_msl, 90)
        client.post_telemetry(telemetry)
Example #14
0
# Tracks the uploads since last print.
sent_lock = threading.Lock()
last_print = datetime.datetime.now()
sent_since_print = 0


# Tracks upload success and prints any exceptions.
def handle_upload_result(future):
    if future.exception():
        print 'Request Failed. Exception: %s' % str(future.exception())
    else:
        with sent_lock:
            sent_since_print += 1

# Continuously poll for new telemetry and send to server.
last_telemetry = Telemetry(0, 0, 0, 0)
while True:
    telemetry = Telemetry(latitude=cs.lat,
                          longitude=cs.lng,
                          altitude=cs.alt,
                          uas_heading=cs.groundcourse)
    if telemetry != last_telemetry:
        client.post_telemetry(telemetry).add_done_callback(
            handle_upload_result)
        last_telemetry = telemetry

    now = datetime.datetime.now()
    since_print = (now - last_print).total_seconds()
    if since_print >= PRINT_SEC:
        with sent_lock:
            local_sent_since_print = sent_since_print