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 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 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)