diff --git a/tools/odrive/discovery.py b/tools/odrive/discovery.py index 745a9e92..7822950a 100644 --- a/tools/odrive/discovery.py +++ b/tools/odrive/discovery.py @@ -7,6 +7,7 @@ import json import time import threading import odrive.protocol +import odrive.utils import odrive.remote_object import odrive.usbbulk_transport import odrive.serial_transport @@ -35,29 +36,35 @@ def find_all(path, serial_number, This queries the endpoint 0 on that channel to gain information about the interface, which is then used to init the corresponding object. """ - printer("Connecting to device on " + channel._name) try: - json_bytes = channel.remote_endpoint_read_buffer(0) - except (odrive.protocol.TimeoutException, odrive.protocol.ChannelBrokenException): - raise odrive.protocol.DeviceInitException("no response - probably incompatible") - json_crc16 = odrive.protocol.calc_crc16(odrive.protocol.PROTOCOL_VERSION, json_bytes) - channel._interface_definition_crc = json_crc16 - try: - json_string = json_bytes.decode("ascii") - except UnicodeDecodeError: - raise odrive.protocol.DeviceInitException("device responded on endpoint 0 with something that is not ASCII") - printer("JSON: " + json_string) - try: - json_data = json.loads(json_string) - except json.decoder.JSONDecodeError as error: - raise odrive.protocol.DeviceInitException("device responded on endpoint 0 with something that is not JSON: " + str(error)) - json_data = {"name": "odrive", "members": json_data} - obj = odrive.remote_object.RemoteObject(json_data, None, channel, None, printer) - device_serial_number = serial_number if hasattr(obj, 'serial_number') else "[unknown serial number]" - if serial_number != None and device_serial_number != serial_number: - printer("Ignoring device with serial number {}".format(device_serial_number)) - return - did_discover_object_callback(obj) + printer("Connecting to device on " + channel._name) + try: + json_bytes = channel.remote_endpoint_read_buffer(0) + except (odrive.utils.TimeoutException, odrive.protocol.ChannelBrokenException): + printer("no response - probably incompatible") + return + json_crc16 = odrive.protocol.calc_crc16(odrive.protocol.PROTOCOL_VERSION, json_bytes) + channel._interface_definition_crc = json_crc16 + try: + json_string = json_bytes.decode("ascii") + except UnicodeDecodeError: + printer("device responded on endpoint 0 with something that is not ASCII") + return + printer("JSON: " + json_string) + try: + json_data = json.loads(json_string) + except json.decoder.JSONDecodeError as error: + printer("device responded on endpoint 0 with something that is not JSON: " + str(error)) + return + json_data = {"name": "odrive", "members": json_data} + obj = odrive.remote_object.RemoteObject(json_data, None, channel, None, printer) + device_serial_number = serial_number if hasattr(obj, 'serial_number') else "[unknown serial number]" + if serial_number != None and device_serial_number != serial_number: + printer("Ignoring device with serial number {}".format(device_serial_number)) + return + did_discover_object_callback(obj) + except Exception as ex: + printer("Unexpected exception after discovering channel: " + str(ex)) # For each connection type, kick off an appropriate discovery loop for search_spec in path.split(','): diff --git a/tools/odrive/protocol.py b/tools/odrive/protocol.py index 4878fadc..a6c326eb 100644 --- a/tools/odrive/protocol.py +++ b/tools/odrive/protocol.py @@ -3,6 +3,10 @@ import time import struct import sys +import threading +import odrive.utils +from odrive.utils import wait_any +from odrive.utils import Event import abc @@ -63,9 +67,6 @@ def calc_crc16(remainder, value): #print(hex(calc_crc16(0xfeef, [1, 2, 3, 4, 5, 0x10, 0x13, 0x37]))) -class TimeoutException(Exception): - pass - class DeviceInitException(Exception): pass @@ -204,11 +205,14 @@ class Channel(PacketSink): _outbound_seq_no = 0 _interface_definition_crc = 0 _expected_acks = {} + _responses = {} # Choose these parameters to be sensible for a specific transport layer _resend_timeout = 0.1 # [s] _send_attempts = 5 + _channel_broken = Event() + def __init__(self, name, input, output): """ Params: @@ -220,6 +224,33 @@ class Channel(PacketSink): self._name = name self._input = input self._output = output + self._my_lock = threading.Lock() + self.start_receiver_thread(Event()) # TODO: use app_shutdown_token + + def start_receiver_thread(self, cancellation_token): + """ + Starts the receiver thread that processes incoming messages. + The thread quits as soon as the channel enters a broken state. + """ + def receiver_thread(): + try: + while (not cancellation_token.is_set()) and (not self._channel_broken.is_set()): + # Set an arbitrary deadline because the get_packet function + # currently doesn't support a cancellation_token + deadline = time.monotonic() + 1.0 + try: + response = self._input.get_packet(deadline) + except odrive.utils.TimeoutException: + continue # try again + except ChannelDamagedException: + continue # try again + # Process response + # This should not throw an exception, otherwise the channel breaks + self.process_packet(response) + print("receiver thread is exiting") + finally: + self._channel_broken.set() + threading.Thread(target=receiver_thread, daemon=True).start() def remote_endpoint_operation(self, endpoint_id, input, expect_ack, output_length): if input is None: @@ -230,9 +261,13 @@ class Channel(PacketSink): if (expect_ack): endpoint_id |= 0x8000 - self._outbound_seq_no = ((self._outbound_seq_no + 1) & 0x7fff) - self._outbound_seq_no |= 0x80 # FIXME: we hardwire one bit of the seq-no to 1 to avoid conflicts with the legacy protocol - seq_no = self._outbound_seq_no + self._my_lock.acquire() + try: + self._outbound_seq_no = ((self._outbound_seq_no + 1) & 0x7fff) + seq_no = self._outbound_seq_no + finally: + self._my_lock.release() + seq_no |= 0x80 # FIXME: we hardwire one bit of the seq-no to 1 to avoid conflicts with the legacy protocol packet = struct.pack('