def init_mavlink(self, master_dev, gcs_dev, extra, baudrate):
# master
master = mavutil.mavserial(master_dev, baud=baudrate, autoreconnect=True)
print 'master connected on device: ', master_dev
# gcs
if gcs_dev is not None:
gcs = mavutil.mavudp(gcs_dev, input=False)
print 'gcs connected on device: ', gcs_dev
self.extra_out = None
if extra:
try:
extra = int(extra)
except:
raise TypeError("extraOut must be int or NONE")
extra_out = mavutil.mavudp(gcs_dev, input=False,
source_system=extra)
print 'Unmodified sensor output connected on device: ',\
gcs_dev, ' with vehicle ID ', extra
self.extra_out = extra_out
# class data
self.master = master
self.gcs = gcs
def _px4_rx_thread(self, sleep_time=0.0005):
"""
This function serves the purpose of receiving messages from the flight controller at such a rate that no buffer
overflow occurs. When mav_device.recv_msg() is called, if enough data has come in from the serial connection to
form a MAVLink packet, the packet is parsed and the latest copy of the particular type of MAVLink message is
updated in the mav_device.messages dict. This dict is used in the main thread for scheduling the transmission
of each type of MAVLink packet, effectively decimating the stream to a set rate for each type of MAVLink message
:param sleep_time: Sleep time between loops
"""
while not self.running:
time.sleep(0.01)
self.px4 = mavutil.mavserial(self.px4_port, PX4_COMPANION_BAUD, source_component=1)
while self.running:
# Rx Message
msg = self.px4.recv_msg()
if not msg: # Nothing received, wait and loop
time.sleep(sleep_time)
continue
# Message received, decide what to do with it
mav_type = msg.get_type()
if mav_type in MAV_IGNORES: # Ignore message
pass
elif mav_type not in self.rates: # Priority Message
self.queue_out.write(msg, priority=True)
logging.info(f'Priority message type: {mav_type}')
elif time.time() >= self.next_times[mav_type]: # Scheduled message
self.next_times[mav_type] = time.time() + self.rates[mav_type]
self.queue_out.write(msg, priority=False)
self.px4.close()
def init_mavlink(self, master_dev, gcs_dev, extra, baudrate):
# master
master = mavutil.mavserial(master_dev,
baud=baudrate,
autoreconnect=True)
print 'master connected on device: ', master_dev
# gcs
if gcs_dev is not None:
gcs = mavutil.mavudp(gcs_dev, input=False)
print 'gcs connected on device: ', gcs_dev
self.extra_out = None
if extra:
try:
extra = int(extra)
except:
raise TypeError("extraOut must be int or NONE")
extra_out = mavutil.mavudp(gcs_dev,
input=False,
source_system=extra)
print 'Unmodified sensor output connected on device: ',\
gcs_dev, ' with vehicle ID ', extra
self.extra_out = extra_out
# class data
self.master = master
self.gcs = gcs
def create_connection(self):
state = self.state
## camera
connection = mavutil.mavudp("127.0.0.1:14676", input= True)
link = mavutil.mavlink.MAVLink(connection)
new_connection = Connection(connection, link, system_id = 122, component_id = 101, input=True)
self.add_connection_to_table(new_connection)
connection = mavutil.mavudp("127.0.0.1:14666", input= False)
link = mavutil.mavlink.MAVLink(connection)
new_connection = Connection(connection, link, system_id = 122, component_id = 100, output=True)
self.add_connection_to_table(new_connection)
## media manager
connection = mavutil.mavudp("127.0.0.1:14677", input= True)
link = mavutil.mavlink.MAVLink(connection)
new_connection = Connection(connection, link, system_id = 122, component_id = 201, input=True)
self.add_connection_to_table(new_connection)
connection = mavutil.mavudp("127.0.0.1:14667", input= False)
link = mavutil.mavlink.MAVLink(connection)
new_connection = Connection(connection, link, system_id = 122, component_id = 200, output=True)
self.add_connection_to_table(new_connection)
## block sender
connection = mavutil.mavudp("127.0.0.1:14678", input= True)
link = mavutil.mavlink.MAVLink(connection)
new_connection = Connection(connection, link, system_id = 122, component_id = 253, input=True)
self.add_connection_to_table(new_connection)
connection = mavutil.mavudp("127.0.0.1:14668", input= False)
link = mavutil.mavlink.MAVLink(connection)
new_connection = Connection(connection, link, system_id = 122, component_id = 254, output=True)
self.add_connection_to_table(new_connection)
## mount (gimbal)
## ap bi-directional
connection = mavutil.mavserial("/dev/serial/by-id/usb-FTDI_FT232R_USB_UART_A4006JAo-if00-port0",baud=57600)
link = mavutil.mavlink.MAVLink(connection)
new_connection = Connection(connection, link, system_id = 122, component_id = 1, input=True, output=True)
self.add_connection_to_table(new_connection)
## ap_state
connection = mavutil.mavudp("127.0.0.1:14779", input= True)
link = mavutil.mavlink.MAVLink(connection)
new_connection = Connection(connection, link, system_id = 122, component_id = 2, input=True)
self.add_connection_to_table(new_connection)
connection = mavutil.mavudp("127.0.0.1:14769", input= False)
link = mavutil.mavlink.MAVLink(connection)
new_connection = Connection(connection, link, system_id = 122, component_id = 3, output=True)
self.add_connection_to_table(new_connection)
def init_mavlink(self, master_dev, gcs_dev, baudrate):
# master
master = mavutil.mavserial(master_dev, baud=baudrate, autoreconnect=True)
print 'master connected on device: ', master_dev
# gcs
if gcs_dev is not None:
gcs = mavutil.mavudp(gcs_dev, input=False)
print 'gcs connected on device: ', gcs_dev
# class data
self.master = master
self.gcs = gcs
def init_mavlink(self, master_dev, gcs_dev, baudrate):
# master
master = mavutil.mavserial(master_dev,
baud=baudrate,
autoreconnect=True)
print 'master connected on device: ', master_dev
# gcs
if gcs_dev is not None:
gcs = mavutil.mavudp(gcs_dev, input=False)
print 'gcs connected on device: ', gcs_dev
# class data
self.master = master
self.gcs = gcs
def listener_noros(self):
import pymavlink.mavutil as mavutil
m=mavutil.mavserial(self.flow_serial)
# wait for a heartbeat, so we are synced with the protocol
m.wait_heartbeat()
#m.messages['OPTICAL_FLOW']
last_print = 0
self.sum_x = 0
self.sum_y = 0
while not self.thread_please_die:
#x = m.recv_msg()
x = m.recv_match(type='OPTICAL_FLOW_RAD', blocking=True)
if x is None:
print 'x is NONE!!'
continue
# TODO:
#sum_x += x.flow_x
self.callback(x)
#print x
if int(time.time()) > last_print:
last_print = int(time.time())
def listener_noros():
import pymavlink.mavutil as mavutil
m=mavutil.mavserial('/dev/ttyACM0')
# wait for a heartbeat, so we are synced with the protocol
m.wait_heartbeat()
#m.messages['OPTICAL_FLOW']
last_print = 0
sum_x = 0
sum_y = 0
while True:
#x = m.recv_msg()
x = m.recv_match(type='OPTICAL_FLOW_RAD', blocking=True)
if x is None:
print 'x is NONE!!'
continue
# TODO:
#sum_x += x.flow_x
callback(x)
#print x
if int(time.time()) > last_print:
last_print = int(time.time())
write = False
append = False
robust_parsing = True
notimestamps = False
input = True,
dialect = None
autoreconnect = False
zero_time_base = False
retries = 3
use_native = False,
force_connected = False
progress_callback = None
# fakePort = mavutil.MavlinkSerialPort("/dev/tty.bruh", 115200)
# master = mavutil.mavserial("/dev/tty.bruh")
master = mavutil.mavserial("/dev/cu.usbmodem01", baud=57600)
print("past")
def handle_heartbeat(msg):
mode = mavutil.mode_string_v10(msg)
is_armed = msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED
is_enabled = msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_GUIDED_ENABLED
def handle_imu(msg):
imu_data = [msg.xacc, msg.yacc, msg.zacc, msg.xgyro, msg.ygyro, msg.zgyro]
print("imu_data", imu_data)
def handle_attitude(msg):
#!/usr/bin/env python
import socket
import serial
import select
from pymavlink import mavutil
source = ("/dev/ttyUSB0", 57600)
bind = ("0.0.0.0", 14555)
dest = [ ("192.168.40.255", 14550) ]
udp_sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
udp_sock.bind((bind[0],bind[1]))
udp_sock.setblocking(0)
udp_sock.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1)
mav = mavutil.mavserial(source[0], baud=source[1], use_native=False)
#with serial.Serial(port=source[0], baudrate=source[1], rtscts=True, timeout=0, writeTimeout=0) as ser:
while True:
readable, writable, exceptional = select.select([mav.fd, udp_sock], [], [], 1)
if udp_sock in readable:
msg = udp_sock.recv(4096)
bytes = mav.write(msg)
#print("From client: %s" % msg)
#print("%i in : %i out" % (len(msg),bytes))
if mav.fd in readable:
msg = mav.recv_msg()
if msg:
#print(msg.get_msgbuf())
for d in dest:
while 1:
try:
import yaml
settings = yaml.load(open('setting.yml'))
mocap_bodies = {}
mocap_pc_ip = settings['mocap_pc_ip']
serial_name = settings['serial_name']
white_list = settings['white_list']
import serial
from xbee import XBee, ZigBee
serial_port = serial.Serial(serial_name, 57600, timeout=0.02)
xbee_device = ZigBee(serial_port)
from pymavlink.mavutil import mavserial
mav = mavserial(serial_name, 57600)
