uds drain before send and use has_obd()

python/uds.py

@@ -213,6 +213,7 @@ class MessageTimeoutError(Exception):
 
 class NegativeResponseError(Exception):
   def __init__(self, message, service_id, error_code):
+    super().__init__()
     self.message = message
     self.service_id = service_id
     self.error_code = error_code
@@ -386,7 +387,7 @@ class UdsClient():
     self.panda = panda
     self.bus = bus
     self.tx_addr = tx_addr
-    if rx_addr == None:
+    if rx_addr is None:
       if tx_addr < 0xFFF8:
         # standard 11 bit response addr (add 8)
         self.rx_addr = tx_addr+8
@@ -407,13 +408,6 @@ class UdsClient():
 
   def _can_thread(self, debug: bool=False):
     try:
-      # allow all output
-      self.panda.set_safety_mode(0x1337)
-      # clear tx buffer
-      self.panda.can_clear(self.bus)
-      # clear rx buffer
-      self.panda.can_clear(0xFFFF)
-
       while True:
         # send
         while not self.can_tx_queue.empty():
@@ -428,13 +422,21 @@ class UdsClient():
             continue
           if debug: print("CAN-RX: {} - {}".format(hex(self.rx_addr), hexlify(rx_data)))
           self.can_rx_queue.put(rx_data)
-        else:
+
+        if len(msgs) == 0:
           time.sleep(0.01)
     finally:
       self.panda.close()
 
   # generic uds request
   def _uds_request(self, service_type: SERVICE_TYPE, subfunction: int=None, data: bytes=None) -> bytes:
+    # throw away any stale data
+    while not self.can_rx_queue.empty():
+      try:
+        self.can_rx_queue.get(block=False)
+      except BaseException:
+        pass
+
     req = bytes([service_type])
     if subfunction is not None:
       req += bytes([subfunction])
@@ -453,12 +455,12 @@ class UdsClient():
         service_id = resp[1] if len(resp) > 1 else -1
         try:
           service_desc = SERVICE_TYPE(service_id).name
-        except Exception:
+        except BaseException:
           service_desc = 'NON_STANDARD_SERVICE'
         error_code = resp[2] if len(resp) > 2 else -1
         try:
           error_desc = _negative_response_codes[error_code]
-        except Exception:
+        except BaseException:
           error_desc = resp[3:]
         # wait for another message if response pending
         if error_code == 0x78: