From 52ec0bd1bd24f5654292de1d38cd8941234fb4ea Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 20 Sep 2016 12:28:46 +1000 Subject: [PATCH 1/2] mavutil: add force-connect option --- mavutil.py | 51 ++++++++++++++++++++++++++++++++++++++++++++------- 1 file changed, 44 insertions(+), 7 deletions(-) diff --git a/mavutil.py b/mavutil.py index 72d0c90da..762a861f1 100644 --- a/mavutil.py +++ b/mavutil.py @@ -790,20 +790,39 @@ def set_close_on_exec(fd): except Exception: pass +class FakeSerial(): + def __init__(self): + pass + def read(self, len): + return "" + def write(self, buf): + raise Exception("write always fails") + def inWaiting(self): + return 0 + def close(self): + pass + class mavserial(mavfile): '''a serial mavlink port''' - def __init__(self, device, baud=115200, autoreconnect=False, source_system=255, source_component=0, use_native=default_native): + def __init__(self, device, baud=115200, autoreconnect=False, source_system=255, source_component=0, use_native=default_native, force_connected=False): import serial if ',' in device and not os.path.exists(device): device, baud = device.split(',') self.baud = baud self.device = device self.autoreconnect = autoreconnect + self.force_connected = force_connected # we rather strangely set the baudrate initially to 1200, then change to the desired # baudrate. This works around a kernel bug on some Linux kernels where the baudrate # is not set correctly - self.port = serial.Serial(self.device, 1200, timeout=0, - dsrdtr=False, rtscts=False, xonxoff=False) + try: + self.port = serial.Serial(self.device, 1200, timeout=0, + dsrdtr=False, rtscts=False, xonxoff=False) + except serial.SerialException as e: + if not force_connected: + raise e + self.port = FakeSerial() + try: fd = self.port.fileno() set_close_on_exec(fd) @@ -856,8 +875,14 @@ def write(self, buf): def reset(self): import serial try: - newport = serial.Serial(self.device, self.baud, timeout=0, - dsrdtr=False, rtscts=False, xonxoff=False) + try: + newport = serial.Serial(self.device, self.baud, timeout=0, + dsrdtr=False, rtscts=False, xonxoff=False) + except serial.SerialException as e: + if not self.force_connected: + raise e + newport = FakeSerial() + return False self.port.close() self.port = newport print("Device %s reopened OK" % self.device) @@ -866,6 +891,7 @@ def reset(self): self.fd = self.port.fileno() except Exception: self.fd = None + self.set_baudrate(self.baud) if self.rtscts: self.set_rtscts(self.rtscts) return True @@ -1229,10 +1255,15 @@ def mavlink_connection(device, baud=115200, source_system=255, source_component= planner_format=None, write=False, append=False, robust_parsing=True, notimestamps=False, input=True, dialect=None, autoreconnect=False, zero_time_base=False, - retries=3, use_native=default_native): + retries=3, use_native=default_native, + force_connected=False): '''open a serial, UDP, TCP or file mavlink connection''' global mavfile_global + if force_connected: + # force_connected implies autoreconnect + autoreconnect = True + if dialect is not None: set_dialect(dialect) if device.startswith('tcp:'): @@ -1282,7 +1313,13 @@ def mavlink_connection(device, baud=115200, source_system=255, source_component= return mavlogfile(device, planner_format=planner_format, write=write, append=append, robust_parsing=robust_parsing, notimestamps=notimestamps, source_system=source_system, source_component=source_component, use_native=use_native) - return mavserial(device, baud=baud, source_system=source_system, source_component=source_component, autoreconnect=autoreconnect, use_native=use_native) + return mavserial(device, + baud=baud, + source_system=source_system, + source_component=source_component, + autoreconnect=autoreconnect, + use_native=use_native, + force_connected=force_connected) class periodic_event(object): '''a class for fixed frequency events''' From 9143db6e6a0a07a8dbd5f1ddff46f72cb873135a Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 12 Jan 2017 22:29:31 +1100 Subject: [PATCH 2/2] mavutil: wait_heartbeat takes a timeout --- mavutil.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/mavutil.py b/mavutil.py index 762a861f1..018bb1e20 100644 --- a/mavutil.py +++ b/mavutil.py @@ -389,9 +389,9 @@ def setup_logfile_raw(self, logfile, mode='w'): '''start logging raw bytes to the given logfile, without timestamps''' self.logfile_raw = open(logfile, mode=mode) - def wait_heartbeat(self, blocking=True): + def wait_heartbeat(self, blocking=True, timeout=None): '''wait for a heartbeat so we know the target system IDs''' - return self.recv_match(type='HEARTBEAT', blocking=blocking) + return self.recv_match(type='HEARTBEAT', blocking=blocking, timeout=timeout) def param_fetch_all(self): '''initiate fetch of all parameters'''