diff --git a/dronecan/driver/__init__.py b/dronecan/driver/__init__.py index 59b669c..07f38ec 100644 --- a/dronecan/driver/__init__.py +++ b/dronecan/driver/__init__.py @@ -32,7 +32,8 @@ def is_mavlink_port(device_name, **kwargs): if not have_mavcan: return False baudrate = kwargs.get('baudrate', 115200) - return MAVCAN.is_mavlink_port(device_name, baudrate) + source_system = kwargs.get('mavlink_source_system', 250) + return MAVCAN.is_mavlink_port(device_name, baudrate, source_system) def make_driver(device_name, **kwargs): """Creates an instance of CAN driver. diff --git a/dronecan/driver/mavcan.py b/dronecan/driver/mavcan.py index 4f0b9bf..0f6ba0a 100644 --- a/dronecan/driver/mavcan.py +++ b/dronecan/driver/mavcan.py @@ -34,7 +34,7 @@ def __init__(self, command, data): self.command = command self.data = data -def io_process(url, bus, target_system, baudrate, tx_queue, rx_queue, exit_queue, parent_pid): +def io_process(url, bus, source_system, target_system, baudrate, tx_queue, rx_queue, exit_queue, parent_pid): os.environ['MAVLINK20'] = '1' target_component = 0 @@ -47,8 +47,8 @@ def io_process(url, bus, target_system, baudrate, tx_queue, rx_queue, exit_queue readonly = False def connect(): - nonlocal conn, baudrate, readonly - conn = mavutil.mavlink_connection(url, baud=baudrate, source_system=250, + nonlocal conn, baudrate, readonly, source_system + conn = mavutil.mavlink_connection(url, baud=baudrate, source_system=source_system, source_component=mavutil.mavlink.MAV_COMP_ID_MAVCAN, dialect='ardupilotmega') if conn is None: @@ -203,6 +203,7 @@ def __init__(self, url, **kwargs): super(MAVCAN, self).__init__() self.bus = kwargs.get('bus_number', 1) - 1 self.target_system = kwargs.get('mavlink_target_system', 0) + self.source_system = kwargs.get('mavlink_source_system', 250) self.filter_list = None baudrate = kwargs.get('baudrate', 115200) @@ -211,7 +212,7 @@ def __init__(self, url, **kwargs): self.exit_queue = multiprocessing.Queue(maxsize=1) self.proc = multiprocessing.Process(target=io_process, name='mavcan_io_process', - args=(url, self.bus, self.target_system, baudrate, + args=(url, self.bus, self.source_system, self.target_system, baudrate, self.tx_queue, self.rx_queue, self.exit_queue, os.getpid())) self.proc.daemon = True self.proc.start() @@ -248,10 +249,10 @@ def send_frame(self, frame): self._tx_hook(frame) self.tx_queue.put_nowait(frame) - def is_mavlink_port(device_name, baudrate): + def is_mavlink_port(device_name, baudrate, source_system): '''check if a device is sending mavlink''' os.environ['MAVLINK20'] = '1' - conn = mavutil.mavlink_connection(device_name, baud=baudrate, source_system=250, source_component=mavutil.mavlink.MAV_COMP_ID_MAVCAN) + conn = mavutil.mavlink_connection(device_name, baud=baudrate, source_system=source_system, source_component=mavutil.mavlink.MAV_COMP_ID_MAVCAN) if not conn: return False m = conn.recv_match(blocking=True, type=['HEARTBEAT','ATTITUDE', 'SYS_STATUS'], timeout=1.1)