def __init__(self, uri, timeout=5): """ Args: uri: address of the websocket server, e.g. "ws://127.0.0.1:5760" timeout: the time limit in seconds to wait for a message prior to closing connection """ # call the superclass constructor super().__init__(threaded=False) self._uri = uri self._f = BytesIO() self._mav = mavlink.MAVLink(self._f) # This will be set with self._uri when the async event loop is # started self._ws = None self._q = asyncio.Queue() # management self._target_system = 1 self._target_component = 1 self._running = False # seconds to wait of no messages before termination self._timeout = timeout
def process_telemetrylog_file(input_file, output_file): desired_messages = get_desired_messages_telemetrylog() #desired_messages = ['WIND', 'AHRS'] with open(input_file, 'rb') as infile, open(output_file, 'w') as outfile: current_message_types = {} message_formats = get_all_message_formats_telemetry_log( desired_messages) mav = mavlink2.MAVLink(infile) time, buff = get_message_buffer(infile, mav) while buff != '': time = str(int(struct.unpack('>Q', time)[0])) mav_message = mav.decode(buff) values = [] for field_name in mav_message.fieldnames: value = getattr(mav_message, field_name) values.append(str(value)) value_string = ','.join(values) line = mav_message.name + ', ' + time + ', ' + value_string message = MessageType(line) if message.name in desired_messages: current_message_types = message.update_message_values( current_message_types) write_output_message_line(desired_messages, current_message_types, message_formats, outfile) time, buff = get_message_buffer(infile, mav) return True
def __init__(self): super(QtCore.QThread, self).__init__() # initialize a MAVLink parser on the same system as QGroundControl, but a different component id self.mav = mavlink.MAVLink(None, srcSystem=255, srcComponent=25, use_native=False)
async def handle_ws(ws, path): global stop f = BytesIO() mav = mavlink.MAVLink(f) while not stop.done(): # hb = mav.heartbeat_encode(mavutil.mavlink.MAV_TYPE_GCS, mavutil.mavlink.MAV_AUTOPILOT_INVALID, 0, 0, # mavutil.mavlink.MAV_STATE_ACTIVE) # ws.send(hb) msg = await ws.recv() mav.decode(bytearray(msg))
def __init__(self): super().__init__() # we need Sources for each source we send # so we can pack messages correctly and keep track of sequence numbers # dictionary of (src_sysid, src_compid) to Source objects self._sources = {} # we also need a proper mavlink object so we can receive and decode # messages from the transport # but it doesn't have a file cause we don't send things with it self._mav = mavlink.MAVLink(None)
async def handle_tcp(r, w): global stop f = BytesIO() mav = mavlink.MAVLink(f) while not stop.done(): # NOTE: Since not guaranteed we'll get each mavlink message # independently we have to manually separate them. msg = await r.read(1024) # print(len(msg), msg) idx = 0 while idx < len(msg): # decode the packet (msg + 10 header + 2 crc), ignoring signature plen = msg[idx + 1] + 10 + 2 mav.decode(bytearray(msg[idx:idx + plen])) idx += plen
def __init__(self, remote_addr, remote_port = 14550): self._BOOTUP_TIME = time.time() self._sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) self._remote_addr = remote_addr self._remote_port = remote_port self._sock.bind(("0.0.0.0", 0)) f = fifo() self._mav = mavlink1.MAVLink(f, srcSystem=1, srcComponent=1) self._last_heartbeat_time = time.time() self._last_attitude_time = time.time() self._last_gps_time = time.time() self._last_vfr_hud_time = time.time() self._last_status_time = time.time() self._heading = 0 self._mission_count = 0 self._mission_items = [] self._mission_items_int = [] self.custom_mode = 0
def __init__(self, name, ip, port, remote_device): """ Initializer :param name: Name for this object :param ip: IP address for MAVLink UDP connection with GCS :param port: Port unique to this vehicle for MAVLink UDP connection with GCS :param remote_device: RemoteXBeeDevice object associated with this UAVs XBee radio """ self.name = name self.ip = ip self.port = port self.queue_in = MAVQueue() self.queue_out = MAVQueue() self.socket = mavutil.mavudp(device=f'{ip}:{port}', input=False) self.parser = mavlink.MAVLink(Fifo()) self.device = remote_device self.connected = True logging.info(f'Assigned {name} link to UDP {(ip, port)}')
def convert(gcs, uav, file): f = fifo() packets = rdpcap(file) mav = mavlink2.MAVLink(f) output = file.replace("pcap", "csv") print(f"Write {output}.\nStarting...") with open(output, "w") as outfile: outfile.write("Timestamp;Src;Dst;MavLinkMsg;MavLinkParam\n") timestamp, dst, src, data = ("", "", "", "") for packet in packets: if packet.getlayer("ICMP") is None and packet.getlayer( "IP") is not None and packet.version == 4: timestamp = time.strftime('%Y-%m-%d %H:%M:%S', time.localtime(int(packet.time))) # Take packet from uav to gcs (only on port 14550) if packet.getlayer("IP").src == uav and packet.getlayer( "IP").dst == gcs and packet.getlayer( "IP").dport == 14550: data = packet.getlayer("Raw").load src, dst = ("UAV", "GCS") # src, dst = (uav, gcs) write ip # Take packet from gcs (only port 14550) to uav elif packet.getlayer("IP").src == gcs and packet.getlayer( "IP").dst == uav and packet.getlayer( "IP").sport == 14550: data = packet.getlayer("Raw").load src, dst = ("GCS", "UAV") # src, dst = (gcs, uav) write ip else: continue #print("Garbage: ", packet.getlayer("Raw").load) type_msg, param = str(mav.decode(bytearray(data))).split('{') outfile.write( f"{timestamp};{src};{dst};{type_msg};{param.split('}')[0]}\n" ) print("Conversion completed")
def __init__(self, settings, udp_str=None, usbcam=False): """ Initializer :param settings: Dict of settings related to the UAV, see uav_settings.json for a template/example :param udp_str: String format for UDP connection target ('IP:PORT') """ # Initialized lists, queues, look-up-tables and counters logging.info(f'PX4 adapter script initialized with udp_str={udp_str}') # TODO Can probably make UAVObject a parent class with a little reworking self.known_endpoints = [] self.old_coordinators = [] self.queue_out = MAVQueue() self.parser = mavlink.MAVLink(Fifo()) self.settings = settings self.rates = settings['mav_rates'] self.next_times = {k: time.time() + self.rates[k] for k in self.rates} self.seq = 0 self.running = False self.px4 = None self.udp_str = udp_str self.px4_port = device_finder('FT232R USB UART') self.usbcam = usbcam
import time from collections import Counter from io import BytesIO import websockets from pymavlink.dialects.v20 import ardupilotmega as mavlink if platform.system() is not 'Windows': import uvloop asyncio.set_event_loop_policy(uvloop.EventLoopPolicy()) logger = logging.getLogger('websockets.server') logger.setLevel(logging.ERROR) logger.addHandler(logging.StreamHandler()) f = BytesIO() mav = mavlink.MAVLink(f) # Keep track of connections. We'll use this to relay # a connection's messages to all the other connections. connections = set() message_rates = Counter() async def relay(ws, path): global connections global message_rates connections.add(ws) prev_time = time.time() while True: try: msg = await ws.recv()
def test_drone_state_update(): f = BytesIO() mav = mavlink.MAVLink(f) c = Connection() d = Drone(c) # http://mavlink.org/messages/common/#HEARTBEAT # msg = mav.heartbeat_encode(mavutil.mavlink.MAV_TYPE_GCS, mavutil.mavlink.MAV_AUTOPILOT_INVALID, 0, 0, # mavutil.mavlink.MAV_STATE_ACTIVE) # http://mavlink.org/messages/common/#GLOBAL_POSITION_INT lat = 37.7749 * 1e7 # degrees * 1e7 lon = 122.4194 * 1e7 # degrees * 1e7 alt = 33.3 * 1000 # millimeters vx = 12.3 * 100 # m/s * 100 vy = 14.3 * 100 # m/s * 100 vz = 18.3 * 100 # m/s * 100 hdg = 88 * 100 # degrees * 100 msg = mav.global_position_int_encode(time.time(), int(lat), int(lon), int(alt), int(alt), int(vx), int(vy), int(vz), int(hdg)) dispatch_message(c, msg) # NOTE: the order switch of longitude and latitude assert np.array_equal( d.global_position, np.array([float(lon) / 1e7, float(lat) / 1e7, float(alt) / 1000])) assert np.array_equal( d.local_velocity, np.array([float(vx) / 100, float(vy) / 100, float(vz) / 100])) # http://mavlink.org/messages/common#LOCAL_POSITION_NED x = 10. y = 20. z = 30. vx = 30. vy = 22. vz = 8. msg = mav.local_position_ned_encode(time.time(), x, y, z, vx, vy, vz) dispatch_message(c, msg) assert np.array_equal(d.local_position, np.array([x, y, z])) assert np.array_equal(d.local_velocity, np.array([vx, vy, vz])) # http://mavlink.org/messages/common#HOME_POSITION home_lat = 37.7749 * 1e7 # degrees * 1e7 home_lon = 122.4194 * 1e7 # degrees * 1e7 home_alt = 0. * 1000 msg = mav.home_position_encode(home_lat, home_lon, home_alt, 0, 0, 0, 0, 0, 0, 0) dispatch_message(c, msg) expect = np.array( [float(home_lon) / 1e7, float(home_lat) / 1e7, float(home_alt) / 1000]) assert np.array_equal(d.global_home, expect) # http://mavlink.org/messages/common#ATTITUDE_QUATERNION # Calculate euler angles with http://quaternions.online/ q1 = 0.56098553 q2 = 0.43045933 q3 = -0.09229596 q4 = 0.70105738 expect_euler = np.deg2rad(np.array([30, -45, 90])) rollspeed = 33 pitchspeed = 88 yawspeed = 47 msg = mav.attitude_quaternion_encode(time.time(), q1, q2, q3, q4, rollspeed, pitchspeed, yawspeed) dispatch_message(c, msg) assert np.isclose(d.gyro_raw, np.array([rollspeed, pitchspeed, yawspeed])).all() assert np.isclose(d.attitude, expect_euler).all()
# 'eq_' # 'istest' # 'make_decorator' # 'nontrivial' # 'nontrivial_all' # 'nottest' # 'ok_' # 'raises' # 'set_trace' # 'timed' # 'trivial' # 'trivial_all' # 'with_setup'] client = swarmmaster.SwarmClient() mav= mavlink2.MAVLink(open('mav.file', 'wb')) class BasicTestSuite(unittest.TestCase): """Basic test cases.""" def test_mav_packer_change_client_id(self): us= swarmmaster.UDPServer() mp = swarmmaster.Mavpacker(us) client.id = 4 #Check initialization to FALSE assert client.mav_id_correct == False #check if value stays false in case of incorrect SYSID PARAM MESSAGE param_id = b'SYSID_THISMAV' param_value = 5.0 #wrong value!