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!
