mirror of
https://github.com/odriverobotics/ODrive.git
synced 2026-08-20 22:14:34 +08:00
move updated ODrive python protocol library to Fibre
This commit is contained in:
Samuel Sadok
@@ -1,12 +1,5 @@
|
||||
|
||||
from fibre.discovery import find_any, find_all
|
||||
from fibre.udp_transport import open_udp
|
||||
from fibre.tcp_transport import open_tcp
|
||||
try:
|
||||
from fibre.usbbulk_transport import open_usb
|
||||
except ModuleNotFoundError:
|
||||
pass
|
||||
try:
|
||||
from fibre.serial_transport import open_serial
|
||||
except ModuleNotFoundError:
|
||||
pass
|
||||
from fibre.utils import Event, Logger
|
||||
from fibre.protocol import ChannelBrokenException, ChannelDamagedException
|
||||
from fibre.shell import launch_shell
|
||||
|
||||
@@ -1,196 +0,0 @@
|
||||
"""
|
||||
Provides functions for the discovery of fibre hubs
|
||||
"""
|
||||
|
||||
import sys
|
||||
import time
|
||||
import json
|
||||
import fibre.protocol
|
||||
import re
|
||||
import time
|
||||
import os
|
||||
import itertools
|
||||
import struct
|
||||
import functools
|
||||
|
||||
def noprint(x):
|
||||
pass
|
||||
|
||||
class SimpleDeviceProperty(property):
|
||||
"""
|
||||
Used internally by dynamically created objects to translate
|
||||
property assignments and fetches into endpoint operations on the
|
||||
object's associated channel
|
||||
"""
|
||||
def __init__(self, channel, id, type, struct_format, can_read, can_write):
|
||||
self._channel = channel
|
||||
self._id = id
|
||||
self._type = type
|
||||
self._struct_format = struct_format
|
||||
property.__init__(self,
|
||||
self.fget if can_read else None,
|
||||
self.fset if can_write else None)
|
||||
|
||||
def fget(self, obj):
|
||||
size = struct.calcsize(self._struct_format)
|
||||
buffer = self._channel.remote_endpoint_operation(self._id, None, True, size)
|
||||
return struct.unpack(self._struct_format, buffer)[0]
|
||||
|
||||
def fset(self, obj, value):
|
||||
value = self._type(value)
|
||||
buffer = struct.pack(self._struct_format, value)
|
||||
# TODO: Currenly we wait for an ack here. Settle on the default guarantee.
|
||||
self._channel.remote_endpoint_operation(self._id, buffer, True, 0)
|
||||
|
||||
def call_remote_function(channel, trigger_id, arg_properties, *args):
|
||||
"""
|
||||
Used internally by the dynamically created objects to translate
|
||||
function calls into endpoint operations on the associated channel
|
||||
"""
|
||||
if (len(arg_properties) != len(args)):
|
||||
raise TypeError("expected {} arguments but have {}".format(len(arg_properties), len(args)))
|
||||
for i in range(len(args)):
|
||||
arg_properties[i].fset(None, args[i])
|
||||
channel.remote_endpoint_operation(trigger_id, None, True, 0)
|
||||
|
||||
def setattr_or_raise_if_undefined(self, name, value):
|
||||
"""
|
||||
If employed as an object's __setattr__ function, this function
|
||||
makes sure that an assignment to an undefined attribute doesn't
|
||||
create a new attribute but instead raises an exception
|
||||
"""
|
||||
# We can't use hasattr here because internally it fetches the property
|
||||
# value, creating unnecessary bus traffic
|
||||
if name in dir(self):
|
||||
object.__setattr__(self, name, value)
|
||||
else:
|
||||
raise TypeError('Cannot set name %r on object of type %s' % (
|
||||
name, self.__class__.__name__))
|
||||
|
||||
def create_property(name, json_data, channel, printer):
|
||||
"""
|
||||
Dynamically creates a property based on a JSON definition
|
||||
"""
|
||||
name = name or "[anonymous]"
|
||||
|
||||
type_str = json_data.get("type", None)
|
||||
if type_str is None:
|
||||
printer("property {} has no specified type".format(name))
|
||||
return None
|
||||
|
||||
if type_str == "float":
|
||||
property_type = float
|
||||
struct_format = "<f"
|
||||
elif type_str == "bool":
|
||||
property_type = bool
|
||||
struct_format = "<?"
|
||||
elif type_str == "int8":
|
||||
property_type = int
|
||||
struct_format = "<b"
|
||||
elif type_str == "uint8":
|
||||
property_type = int
|
||||
struct_format = "<B"
|
||||
elif type_str == "int16":
|
||||
property_type = int
|
||||
struct_format = "<h"
|
||||
elif type_str == "uint16":
|
||||
property_type = int
|
||||
struct_format = "<H"
|
||||
elif type_str == "int32":
|
||||
property_type = int
|
||||
struct_format = "<i"
|
||||
elif type_str == "uint32":
|
||||
property_type = int
|
||||
struct_format = "<I"
|
||||
else:
|
||||
printer("property {} has unsupported type {}".format(name, type_str))
|
||||
return None
|
||||
|
||||
id_str = json_data.get("id", None)
|
||||
if id_str is None:
|
||||
printer("property {} has no specified ID".format(name))
|
||||
return None
|
||||
|
||||
access_mode = json_data.get("access", "r")
|
||||
return SimpleDeviceProperty(channel, id_str, property_type,
|
||||
struct_format,
|
||||
'r' in access_mode,
|
||||
'w' in access_mode)
|
||||
|
||||
def create_function(name, json_data, channel, printer):
|
||||
"""
|
||||
Dynamically creates a function based on a JSON definition
|
||||
"""
|
||||
id_str = json_data.get("id", None)
|
||||
if id_str is None:
|
||||
printer("function {} has no specified ID".format(name))
|
||||
return None
|
||||
|
||||
inputs = []
|
||||
for param in json_data.get("arguments", []):
|
||||
param["mode"] = "r"
|
||||
inputs.append(create_property(json_data["name"], param, channel, printer))
|
||||
return functools.partial(call_remote_function, channel, id_str, inputs)
|
||||
|
||||
def create_object(name, json_data, namespace, channel, printer=noprint):
|
||||
"""
|
||||
Creates an object that implements the specified JSON type description by
|
||||
communicating with the provided device object
|
||||
"""
|
||||
if not namespace is None:
|
||||
namespace = namespace + "." + name
|
||||
else:
|
||||
namespace = name
|
||||
|
||||
# Build attribute list from JSON
|
||||
attributes = {"__setattr__": setattr_or_raise_if_undefined}
|
||||
for member in json_data.get("members", []):
|
||||
member_name = member.get("name", None)
|
||||
if member_name is None:
|
||||
printer("ignoring unnamed attribute in {}".format(namespace))
|
||||
continue
|
||||
|
||||
type_str = member.get("type", None)
|
||||
if type_str is None:
|
||||
printer("member {} has no specified type".format(member_name))
|
||||
continue
|
||||
|
||||
if type_str == "object":
|
||||
attribute = create_object(member_name, member, namespace, channel, printer=printer)
|
||||
elif type_str == "function":
|
||||
attribute = create_function(member_name, member, channel, printer)
|
||||
else:
|
||||
attribute = create_property(member_name, member, channel, printer)
|
||||
|
||||
if not attribute is None:
|
||||
attributes[member_name] = attribute
|
||||
|
||||
# Create a type from the property list and instantiate it
|
||||
jit_type = type(namespace, (object,), attributes)
|
||||
new_object = jit_type()
|
||||
return new_object
|
||||
|
||||
def object_from_channel(channel, printer=noprint):
|
||||
"""
|
||||
Inits an object from a given channel.
|
||||
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 (fibre.protocol.TimeoutException, fibre.protocol.ChannelBrokenException):
|
||||
raise fibre.protocol.DeviceInitException("no response - probably incompatible")
|
||||
json_crc16 = fibre.protocol.calc_crc16(fibre.protocol.PROTOCOL_VERSION, json_bytes)
|
||||
channel._interface_definition_crc = json_crc16
|
||||
try:
|
||||
json_string = json_bytes.decode("ascii")
|
||||
except UnicodeDecodeError:
|
||||
raise fibre.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:
|
||||
raise fibre.protocol.DeviceInitException("device responded on endpoint 0 with something that is not JSON")
|
||||
json_data = {"name": "fibrehub", "members": json_data}
|
||||
return create_object("fibrehub", json_data, None, channel, printer=printer)
|
||||
@@ -1,69 +1,124 @@
|
||||
"""
|
||||
Provides functions for the discovery of fibre hubs
|
||||
Provides functions for the discovery of Fibre nodes
|
||||
"""
|
||||
|
||||
import sys
|
||||
import time
|
||||
import json
|
||||
#import usb.core
|
||||
#import usb.util
|
||||
#import serial
|
||||
#import serial.tools.list_ports
|
||||
import re
|
||||
import time
|
||||
import os
|
||||
import itertools
|
||||
import struct
|
||||
import functools
|
||||
|
||||
import threading
|
||||
import traceback
|
||||
import fibre.protocol
|
||||
# TODO: refactor code for each transport layer
|
||||
import fibre.udp_transport
|
||||
import fibre.tcp_transport
|
||||
import fibre.utils
|
||||
import fibre.remote_object
|
||||
import fibre.serial_transport
|
||||
from fibre.utils import Event
|
||||
from fibre.protocol import ChannelBrokenException
|
||||
|
||||
# Load all installed transport layers
|
||||
|
||||
channel_types = {}
|
||||
|
||||
try:
|
||||
import fibre.usbbulk_transport
|
||||
channel_types['usb'] = fibre.usbbulk_transport.discover_channels
|
||||
except ModuleNotFoundError:
|
||||
def find_usb_channels():
|
||||
return []
|
||||
pass
|
||||
|
||||
try:
|
||||
import fibre.serial_transport
|
||||
channel_types['serial'] = fibre.serial_transport.discover_channels
|
||||
except ModuleNotFoundError:
|
||||
def find_serial_channels():
|
||||
return []
|
||||
pass
|
||||
|
||||
def noprint(x):
|
||||
pass
|
||||
try:
|
||||
import fibre.tcp_transport
|
||||
channel_types['tcp'] = fibre.tcp_transport.discover_channels
|
||||
except ModuleNotFoundError:
|
||||
pass
|
||||
|
||||
def find_all(consider_usb=True, consider_serial=False, printer=noprint, device_stdout=noprint):
|
||||
try:
|
||||
import fibre.udp_transport
|
||||
channel_types['udp'] = fibre.udp_transport.discover_channels
|
||||
except ModuleNotFoundError:
|
||||
pass
|
||||
|
||||
def noprint(text):
|
||||
pass
|
||||
|
||||
def find_all(path, serial_number,
|
||||
did_discover_object_callback,
|
||||
search_cancellation_token,
|
||||
channel_termination_token,
|
||||
logger):
|
||||
"""
|
||||
Returns a generator with all the connected devices that speak the Fibre protocol
|
||||
Starts scanning for Fibre nodes that match the specified path spec and calls
|
||||
the callback for each Fibre node that is found.
|
||||
This function is non-blocking.
|
||||
"""
|
||||
channels = iter(())
|
||||
if (consider_usb):
|
||||
channels = itertools.chain(channels, find_usb_channels(printer=printer, device_stdout=device_stdout))
|
||||
if (consider_serial):
|
||||
channels = itertools.chain(channels, find_serial_channels(printer=printer, device_stdout=device_stdout))
|
||||
for channel in channels:
|
||||
# TODO: blacklist known bad channels
|
||||
|
||||
def did_discover_channel(channel):
|
||||
"""
|
||||
Inits an object from a given channel and then calls did_discover_object_callback
|
||||
with the created object
|
||||
This queries the endpoint 0 on that channel to gain information
|
||||
about the interface, which is then used to init the corresponding object.
|
||||
"""
|
||||
try:
|
||||
yield object_from_channel(channel, printer)
|
||||
except fibre.protocol.DeviceInitException as ex:
|
||||
printer(str(ex))
|
||||
continue
|
||||
logger.debug("Connecting to device on " + channel._name)
|
||||
try:
|
||||
json_bytes = channel.remote_endpoint_read_buffer(0)
|
||||
except (TimeoutError, ChannelBrokenException):
|
||||
logger.debug("no response - probably incompatible")
|
||||
return
|
||||
json_crc16 = fibre.protocol.calc_crc16(fibre.protocol.PROTOCOL_VERSION, json_bytes)
|
||||
channel._interface_definition_crc = json_crc16
|
||||
try:
|
||||
json_string = json_bytes.decode("ascii")
|
||||
except UnicodeDecodeError:
|
||||
logger.debug("device responded on endpoint 0 with something that is not ASCII")
|
||||
return
|
||||
logger.debug("JSON: " + json_string)
|
||||
logger.debug("JSON checksum: 0x{:02X} 0x{:02X}".format(json_crc16 & 0xff, (json_crc16 >> 8) & 0xff))
|
||||
try:
|
||||
json_data = json.loads(json_string)
|
||||
except json.decoder.JSONDecodeError as error:
|
||||
logger.debug("device responded on endpoint 0 with something that is not JSON: " + str(error))
|
||||
return
|
||||
json_data = {"name": "fibre_node", "members": json_data}
|
||||
obj = fibre.remote_object.RemoteObject(json_data, None, channel, logger)
|
||||
device_serial_number = format(obj.serial_number, 'x').upper() if hasattr(obj, 'serial_number') else "[unknown serial number]"
|
||||
if serial_number != None and device_serial_number != serial_number:
|
||||
logger.debug("Ignoring device with serial number {}".format(device_serial_number))
|
||||
return
|
||||
did_discover_object_callback(obj)
|
||||
except Exception:
|
||||
logger.debug("Unexpected exception after discovering channel: " + traceback.format_exc())
|
||||
|
||||
# For each connection type, kick off an appropriate discovery loop
|
||||
for search_spec in path.split(','):
|
||||
prefix = search_spec.split(':')[0]
|
||||
the_rest = ':'.join(search_spec.split(':')[1:])
|
||||
if prefix in channel_types:
|
||||
threading.Thread(target=channel_types[prefix],
|
||||
args=(the_rest, serial_number, did_discover_channel, search_cancellation_token, channel_termination_token, logger)).start()
|
||||
else:
|
||||
raise Exception("Invalid path spec \"{}\"".format(search_spec))
|
||||
|
||||
|
||||
def find_any(consider_usb=True, consider_serial=False, printer=noprint, device_stdout=noprint):
|
||||
def find_any(path="usb", serial_number=None,
|
||||
search_cancellation_token=None, channel_termination_token=None,
|
||||
timeout=None, printer=noprint):
|
||||
"""
|
||||
Scans for Fibre Hubs on all supported interfaces and returns the first device
|
||||
that is found. If no device is connected the function blocks.
|
||||
Blocks until the first matching Fibre node is connected and then returns that node
|
||||
"""
|
||||
# TODO: do device discovery and instantiation in a separate thread and just wait on a semaphore here
|
||||
|
||||
# poll for device
|
||||
printer("looking for Fibre Hubs...")
|
||||
while True:
|
||||
dev = next(find_all(consider_usb, consider_serial, printer=printer, device_stdout=device_stdout), None)
|
||||
if dev is not None:
|
||||
return dev
|
||||
printer("no device found")
|
||||
time.sleep(1)
|
||||
result = [ None ]
|
||||
done_signal = Event(search_cancellation_token)
|
||||
def did_discover_object(obj):
|
||||
result[0] = obj
|
||||
done_signal.set()
|
||||
find_all(path, serial_number, did_discover_object, done_signal, channel_termination_token, printer)
|
||||
try:
|
||||
done_signal.wait(timeout=timeout)
|
||||
finally:
|
||||
done_signal.set() # terminate find_all
|
||||
return result[0]
|
||||
|
||||
@@ -2,7 +2,21 @@
|
||||
|
||||
import time
|
||||
import struct
|
||||
from abc import ABC, abstractmethod
|
||||
import sys
|
||||
import threading
|
||||
import traceback
|
||||
#import fibre.utils
|
||||
from fibre.utils import Event, wait_any
|
||||
|
||||
import abc
|
||||
if sys.version_info >= (3, 4):
|
||||
ABC = abc.ABC
|
||||
else:
|
||||
ABC = abc.ABCMeta('ABC', (), {})
|
||||
|
||||
if sys.version_info < (3, 3):
|
||||
from monotonic import monotonic
|
||||
time.monotonic = monotonic
|
||||
|
||||
SYNC_BYTE = 0xAA
|
||||
CRC8_INIT = 0x42
|
||||
@@ -30,6 +44,8 @@ def calc_crc(remainder, value, polynomial, bitwidth):
|
||||
def calc_crc8(remainder, value):
|
||||
if isinstance(value, bytearray) or isinstance(value, bytes) or isinstance(value, list):
|
||||
for byte in value:
|
||||
if not isinstance(byte,int):
|
||||
byte = ord(byte)
|
||||
remainder = calc_crc(remainder, byte, CRC8_DEFAULT, 8)
|
||||
else:
|
||||
remainder = calc_crc(remainder, byte, CRC8_DEFAULT, 8)
|
||||
@@ -38,6 +54,8 @@ def calc_crc8(remainder, value):
|
||||
def calc_crc16(remainder, value):
|
||||
if isinstance(value, bytearray) or isinstance(value, bytes) or isinstance(value, list):
|
||||
for byte in value:
|
||||
if not isinstance(byte, int):
|
||||
byte = ord(byte)
|
||||
remainder = calc_crc(remainder, byte, CRC16_DEFAULT, 16)
|
||||
else:
|
||||
remainder = calc_crc(remainder, value, CRC16_DEFAULT, 16)
|
||||
@@ -48,43 +66,49 @@ def calc_crc16(remainder, value):
|
||||
#print(hex(calc_crc16(0xfeef, [1, 2, 3, 4, 5, 0x10, 0x13, 0x37])))
|
||||
|
||||
|
||||
class TimeoutException(Exception):
|
||||
class DeviceInitException(Exception):
|
||||
pass
|
||||
|
||||
class ChannelDamagedException(Exception):
|
||||
"""
|
||||
Raised when the channel is temporarily broken and a
|
||||
resend of the message might be successful
|
||||
"""
|
||||
pass
|
||||
|
||||
class ChannelBrokenException(Exception):
|
||||
pass
|
||||
|
||||
class DeviceInitException(Exception):
|
||||
"""
|
||||
Raised when the channel is permanently broken
|
||||
"""
|
||||
pass
|
||||
|
||||
|
||||
class StreamSource(ABC):
|
||||
@abstractmethod
|
||||
@abc.abstractmethod
|
||||
def get_bytes(self, n_bytes, deadline):
|
||||
pass
|
||||
|
||||
class StreamSink(ABC):
|
||||
@abstractmethod
|
||||
@abc.abstractmethod
|
||||
def process_bytes(self, bytes):
|
||||
pass
|
||||
|
||||
class PacketSource(ABC):
|
||||
@abstractmethod
|
||||
@abc.abstractmethod
|
||||
def get_packet(self, deadline):
|
||||
pass
|
||||
|
||||
class PacketSink(ABC):
|
||||
@abstractmethod
|
||||
@abc.abstractmethod
|
||||
def process_packet(self, packet):
|
||||
pass
|
||||
|
||||
|
||||
class StreamToPacketSegmenter(StreamSink):
|
||||
_header = []
|
||||
_packet = []
|
||||
_packet_length = 0
|
||||
|
||||
def __init__(self, output):
|
||||
self._header = []
|
||||
self._packet = []
|
||||
self._packet_length = 0
|
||||
self._output = output
|
||||
|
||||
def process_bytes(self, bytes):
|
||||
@@ -140,9 +164,8 @@ class StreamBasedPacketSink(PacketSink):
|
||||
self._output.process_bytes(struct.pack('>H', crc16))
|
||||
|
||||
class PacketFromStreamConverter(PacketSource):
|
||||
def __init__(self, input, trash):
|
||||
def __init__(self, input):
|
||||
self._input = input
|
||||
self._trash = trash
|
||||
|
||||
def get_packet(self, deadline):
|
||||
"""
|
||||
@@ -157,19 +180,16 @@ class PacketFromStreamConverter(PacketSource):
|
||||
header = header + self._input.get_bytes_or_fail(1, deadline)
|
||||
if (header[0] != SYNC_BYTE):
|
||||
#print("sync byte mismatch")
|
||||
self._trash(header)
|
||||
continue
|
||||
|
||||
header = header + self._input.get_bytes_or_fail(1, deadline)
|
||||
if (header[1] & 0x80):
|
||||
#print("packet too large")
|
||||
self._trash(header)
|
||||
continue # TODO: support packets larger than 128 bytes
|
||||
|
||||
header = header + self._input.get_bytes_or_fail(1, deadline)
|
||||
if calc_crc8(CRC8_INIT, header) != 0:
|
||||
#print("crc8 mismatch")
|
||||
self._trash(header)
|
||||
continue
|
||||
|
||||
packet_length = header[1] + 2
|
||||
@@ -177,22 +197,16 @@ class PacketFromStreamConverter(PacketSource):
|
||||
packet = self._input.get_bytes_or_fail(packet_length, deadline)
|
||||
if calc_crc16(CRC16_INIT, packet) != 0:
|
||||
#print("crc16 mismatch")
|
||||
self._trash(header)
|
||||
self._trash(packet)
|
||||
continue
|
||||
return packet[:-2]
|
||||
|
||||
|
||||
class Channel(PacketSink):
|
||||
_outbound_seq_no = 0
|
||||
_interface_definition_crc = 0
|
||||
_expected_acks = {}
|
||||
|
||||
# Choose these parameters to be sensible for a specific transport layer
|
||||
_resend_timeout = 5.0 # [s]
|
||||
_send_attempts = 5
|
||||
|
||||
def __init__(self, name, input, output, trash):
|
||||
def __init__(self, name, input, output, cancellation_token, logger):
|
||||
"""
|
||||
Params:
|
||||
input: A PacketSource where this channel will source packets from on
|
||||
@@ -203,7 +217,46 @@ class Channel(PacketSink):
|
||||
self._name = name
|
||||
self._input = input
|
||||
self._output = output
|
||||
self._trash = trash
|
||||
self._logger = logger
|
||||
self._outbound_seq_no = 0
|
||||
self._interface_definition_crc = 0
|
||||
self._expected_acks = {}
|
||||
self._responses = {}
|
||||
self._my_lock = threading.Lock()
|
||||
self._channel_broken = Event(cancellation_token)
|
||||
self.start_receiver_thread(Event(self._channel_broken))
|
||||
|
||||
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():
|
||||
error_ctr = 0
|
||||
try:
|
||||
while (not cancellation_token.is_set() and not self._channel_broken.is_set()
|
||||
and error_ctr < 10):
|
||||
# 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 TimeoutError:
|
||||
continue # try again
|
||||
except ChannelDamagedException:
|
||||
error_ctr += 1
|
||||
continue # try again
|
||||
if (error_ctr > 0):
|
||||
error_ctr -= 1
|
||||
# Process response
|
||||
# This should not throw an exception, otherwise the channel breaks
|
||||
self.process_packet(response)
|
||||
#print("receiver thread is exiting")
|
||||
except Exception:
|
||||
self._logger.debug("receiver thread is exiting: " + traceback.format_exc())
|
||||
finally:
|
||||
self._channel_broken.set()
|
||||
threading.Thread(target=receiver_thread).start()
|
||||
|
||||
def remote_endpoint_operation(self, endpoint_id, input, expect_ack, output_length):
|
||||
if input is None:
|
||||
@@ -214,9 +267,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 ascii protocol
|
||||
packet = struct.pack('<HHH', seq_no, endpoint_id, output_length)
|
||||
packet = packet + input
|
||||
|
||||
@@ -229,25 +286,32 @@ class Channel(PacketSink):
|
||||
packet = packet + struct.pack('<H', trailer)
|
||||
|
||||
if (expect_ack):
|
||||
self._expected_acks[seq_no] = None
|
||||
attempt = 0
|
||||
while (attempt < self._send_attempts):
|
||||
self._output.process_packet(packet)
|
||||
deadline = time.monotonic() + self._resend_timeout
|
||||
# Read and process packets until we get an ack or need to resend
|
||||
# TODO: support I/O driven reception (wait on semaphore)
|
||||
while True:
|
||||
ack_event = Event()
|
||||
self._expected_acks[seq_no] = ack_event
|
||||
try:
|
||||
attempt = 0
|
||||
while (attempt < self._send_attempts):
|
||||
self._my_lock.acquire()
|
||||
try:
|
||||
response = self._input.get_packet(deadline)
|
||||
except TimeoutException:
|
||||
break # resend
|
||||
# process response, which is hopefully our ACK
|
||||
self.process_packet(response)
|
||||
if not self._expected_acks[seq_no] is None:
|
||||
return self._expected_acks.pop(seq_no, None)
|
||||
# TODO: record channel statistics
|
||||
attempt += 1
|
||||
raise ChannelBrokenException()
|
||||
self._output.process_packet(packet)
|
||||
except ChannelDamagedException:
|
||||
attempt += 1
|
||||
continue # resend
|
||||
finally:
|
||||
self._my_lock.release()
|
||||
# Wait for ACK until the resend timeout is exceeded
|
||||
try:
|
||||
if wait_any(self._resend_timeout, ack_event, self._channel_broken) != 0:
|
||||
raise ChannelBrokenException()
|
||||
except TimeoutError:
|
||||
attempt += 1
|
||||
continue # resend
|
||||
return self._responses.pop(seq_no)
|
||||
# TODO: record channel statistics
|
||||
raise ChannelBrokenException() # Too many resend attempts
|
||||
finally:
|
||||
self._expected_acks.pop(seq_no)
|
||||
self._responses.pop(seq_no, None)
|
||||
else:
|
||||
# fire and forget
|
||||
self._output.process_packet(packet)
|
||||
@@ -260,7 +324,7 @@ class Channel(PacketSink):
|
||||
# TODO: handle device that could (maliciously) send infinite stream
|
||||
buffer = bytes()
|
||||
while True:
|
||||
chunk_length = 64
|
||||
chunk_length = 512
|
||||
chunk = self.remote_endpoint_operation(endpoint_id, struct.pack("<I", len(buffer)), True, chunk_length)
|
||||
if (len(chunk) == 0):
|
||||
break
|
||||
@@ -277,13 +341,16 @@ class Channel(PacketSink):
|
||||
|
||||
if (seq_no & 0x8000):
|
||||
seq_no &= 0x7fff
|
||||
self._expected_acks[seq_no] = packet[2:]
|
||||
ack_signal = self._expected_acks.get(seq_no, None)
|
||||
if (ack_signal):
|
||||
self._responses[seq_no] = packet[2:]
|
||||
ack_signal.set()
|
||||
#print("received ack for packet " + str(seq_no))
|
||||
else:
|
||||
print("received unexpected ACK: " + str(seq_no))
|
||||
|
||||
else:
|
||||
#if (calc_crc16(crc16, struct.pack('<HBB', PROTOCOL_VERSION, packet[-2], packet[-1]))):
|
||||
# raise Exception("CRC16 mismatch")
|
||||
#print("endpoint requested")
|
||||
# FIXME: we use non-ack packets to detect printf output, this is really hacky
|
||||
# In the future there should be a dedicated stdout endpoint.
|
||||
self._trash(packet)
|
||||
#if (calc_crc16(CRC16_INIT, struct.pack('<HBB', PROTOCOL_VERSION, packet[-2], packet[-1]))):
|
||||
# raise Exception("CRC16 mismatch")
|
||||
print("endpoint requested")
|
||||
# TODO: handle local endpoint operation
|
||||
|
||||
@@ -1,17 +1,12 @@
|
||||
"""
|
||||
Provides functions for the discovery of ODrive devices
|
||||
Provides functions for the discovery of Fibre nodes
|
||||
"""
|
||||
|
||||
import sys
|
||||
import json
|
||||
import struct
|
||||
import threading
|
||||
import odrive.protocol
|
||||
|
||||
#class ObjectDisappearedError(Exception):
|
||||
# def __init__(self, channel):
|
||||
# self._obj = obj
|
||||
# pass
|
||||
import fibre.protocol
|
||||
|
||||
class ObjectDefinitionError(Exception):
|
||||
pass
|
||||
@@ -85,7 +80,7 @@ class RemoteProperty():
|
||||
# TODO: Currenly we wait for an ack here. Settle on the default guarantee.
|
||||
self._parent.__channel__.remote_endpoint_operation(self._id, buffer, True, 0)
|
||||
|
||||
def dump(self):
|
||||
def _dump(self):
|
||||
if self._name == "serial_number":
|
||||
# special case: serial number should be displayed in hex (TODO: generalize)
|
||||
val_str = "{:012X}".format(self.get_value())
|
||||
@@ -130,14 +125,14 @@ class RemoteFunction(object):
|
||||
if len(self._outputs) > 0:
|
||||
return self._outputs[0].get_value()
|
||||
|
||||
def dump(self):
|
||||
def _dump(self):
|
||||
return "{}({})".format(self._name, ", ".join("{}: {}".format(x._name, x._property_type.__name__) for x in self._inputs))
|
||||
|
||||
class RemoteObject(object):
|
||||
"""
|
||||
Object with functions and properties that map to remote endpoints
|
||||
"""
|
||||
def __init__(self, json_data, parent, channel, printer):
|
||||
def __init__(self, json_data, parent, channel, logger):
|
||||
"""
|
||||
Creates an object that implements the specified JSON type description by
|
||||
communicating over the provided channel
|
||||
@@ -156,13 +151,13 @@ class RemoteObject(object):
|
||||
for member_json in json_data.get("members", []):
|
||||
member_name = member_json.get("name", None)
|
||||
if member_name is None:
|
||||
printer("ignoring unnamed attribute")
|
||||
logger.debug("ignoring unnamed attribute")
|
||||
continue
|
||||
|
||||
try:
|
||||
type_str = member_json.get("type", None)
|
||||
if type_str == "object":
|
||||
attribute = RemoteObject(member_json, self, channel, printer)
|
||||
attribute = RemoteObject(member_json, self, channel, logger)
|
||||
elif type_str == "function":
|
||||
attribute = RemoteFunction(member_json, self)
|
||||
elif type_str != None:
|
||||
@@ -170,7 +165,7 @@ class RemoteObject(object):
|
||||
else:
|
||||
raise ObjectDefinitionError("no type information")
|
||||
except ObjectDefinitionError as ex:
|
||||
printer("malformed member {}: {}".format(member_name, str(ex)))
|
||||
logger.debug("malformed member {}: {}".format(member_name, str(ex)))
|
||||
continue
|
||||
|
||||
self._remote_attributes[member_name] = attribute
|
||||
@@ -181,20 +176,20 @@ class RemoteObject(object):
|
||||
self.__sealed__ = True
|
||||
channel._channel_broken.subscribe(self._tear_down)
|
||||
|
||||
def dump(self, indent, depth):
|
||||
def _dump(self, indent, depth):
|
||||
if depth <= 0:
|
||||
return "..."
|
||||
lines = []
|
||||
for key, val in self._remote_attributes.items():
|
||||
if isinstance(val, RemoteObject):
|
||||
val_str = indent + key + (": " if depth == 1 else ":\n") + val.dump(indent + " ", depth - 1)
|
||||
val_str = indent + key + (": " if depth == 1 else ":\n") + val._dump(indent + " ", depth - 1)
|
||||
else:
|
||||
val_str = indent + val.dump()
|
||||
val_str = indent + val._dump()
|
||||
lines.append(val_str)
|
||||
return "\n".join(lines)
|
||||
|
||||
def __str__(self):
|
||||
return self.dump("", depth=2)
|
||||
return self._dump("", depth=2)
|
||||
|
||||
def __repr__(self):
|
||||
return self.__str__()
|
||||
@@ -3,12 +3,16 @@ Provides classes that implement the StreamSource/StreamSink and
|
||||
PacketSource/PacketSink interfaces for serial ports.
|
||||
"""
|
||||
|
||||
import serial
|
||||
import os
|
||||
import re
|
||||
import time
|
||||
import traceback
|
||||
import serial
|
||||
import serial.tools.list_ports
|
||||
import fibre
|
||||
|
||||
def noprint(x):
|
||||
pass
|
||||
# TODO: make this customizable
|
||||
DEFAULT_BAUDRATE = 115200
|
||||
|
||||
class SerialStreamTransport(fibre.protocol.StreamSource, fibre.protocol.StreamSink):
|
||||
def __init__(self, port, baud):
|
||||
@@ -33,59 +37,64 @@ class SerialStreamTransport(fibre.protocol.StreamSource, fibre.protocol.StreamSi
|
||||
def get_bytes_or_fail(self, n_bytes, deadline):
|
||||
result = self.get_bytes(n_bytes, deadline)
|
||||
if len(result) < n_bytes:
|
||||
raise fibre.protocol.TimeoutException("expected {} bytes but got only {}", n_bytes, len(result))
|
||||
raise TimeoutError("expected {} bytes but got only {}", n_bytes, len(result))
|
||||
return result
|
||||
|
||||
# TODO: provide SerialPacketTransport
|
||||
def close(self):
|
||||
self._dev.close()
|
||||
|
||||
|
||||
|
||||
def channel_from_serial_port(port, baud, packet_based, printer=noprint, device_stdout=noprint):
|
||||
"""
|
||||
Inits a Fibre Protocol channel from a serial port name and baudrate.
|
||||
"""
|
||||
if packet_based == True:
|
||||
# TODO: implement packet based transport over serial
|
||||
raise NotImplementedError("not supported yet")
|
||||
serial_device = fibre.serial_transport.SerialStreamTransport(port, baud)
|
||||
input_stream = fibre.protocol.PacketFromStreamConverter(serial_device, device_stdout)
|
||||
output_stream = fibre.protocol.StreamBasedPacketSink(serial_device)
|
||||
return fibre.protocol.Channel(
|
||||
"serial port {}@{}".format(port, baud),
|
||||
input_stream, output_stream,
|
||||
device_stdout)
|
||||
|
||||
def find_dev_serial_ports(search_regex):
|
||||
def find_dev_serial_ports():
|
||||
try:
|
||||
return ['/dev/' + x for x in filter(re.compile(search_regex).search, os.listdir('/dev'))]
|
||||
return ['/dev/' + x for x in os.listdir('/dev')]
|
||||
except FileNotFoundError:
|
||||
return []
|
||||
|
||||
def find_pyserial_ports():
|
||||
return [x.name for x in serial.tools.list_ports.comports()]
|
||||
return [x.device for x in serial.tools.list_ports.comports()]
|
||||
|
||||
def find_serial_channels(printer=noprint, device_stdout=noprint):
|
||||
|
||||
def discover_channels(path, serial_number, callback, cancellation_token, channel_termination_token, logger):
|
||||
"""
|
||||
Scans for serial ports.
|
||||
Returns a generator of fibre.protocol.Channel objects.
|
||||
Not every returned object necessarily represents a compatible device.
|
||||
Scans for serial ports that match the path spec.
|
||||
This function blocks until cancellation_token is set.
|
||||
Channels spawned by this function run until channel_termination_token is set.
|
||||
"""
|
||||
if path == None:
|
||||
# This regex should match all desired port names on macOS,
|
||||
# Linux and Windows but might match some incorrect port names.
|
||||
regex = r'^(/dev/tty\.usbmodem.*|/dev/ttyACM.*|COM[0-9]+)$'
|
||||
else:
|
||||
regex = "^" + path + "$"
|
||||
|
||||
# Real serial ports or USB-Serial converters (tested on Linux and Windows)
|
||||
real_serial_ports = find_pyserial_ports()
|
||||
known_devices = []
|
||||
def device_matcher(port_name):
|
||||
if port_name in known_devices:
|
||||
return False
|
||||
return bool(re.match(regex, port_name))
|
||||
|
||||
# Serial devices that are exposed by the platform
|
||||
# for the device's USB connection
|
||||
linux_usb_serial_ports = find_dev_serial_ports(r'^ttyACM')
|
||||
macos_usb_serial_ports = find_dev_serial_ports(r'^tty\.usbmodem')
|
||||
def did_disconnect(port_name, device):
|
||||
device.close()
|
||||
# TODO: yes there is a race condition here in case you wonder.
|
||||
known_devices.pop(known_devices.index(port_name))
|
||||
|
||||
for port in real_serial_ports + linux_usb_serial_ports + macos_usb_serial_ports:
|
||||
try:
|
||||
yield channel_from_serial_port(port, 115200, False, printer, device_stdout=device_stdout)
|
||||
except serial.serialutil.SerialException:
|
||||
printer("could not open " + port)
|
||||
continue
|
||||
|
||||
def open_serial(port_name, printer=noprint, device_stdout=noprint):
|
||||
channel = channel_from_serial_port(port_name, 115200, False, printer, device_stdout)
|
||||
return object_from_channel(channel, printer)
|
||||
while not cancellation_token.is_set():
|
||||
all_ports = find_pyserial_ports() + find_dev_serial_ports()
|
||||
new_ports = filter(device_matcher, all_ports)
|
||||
for port_name in new_ports:
|
||||
try:
|
||||
serial_device = SerialStreamTransport(port_name, DEFAULT_BAUDRATE)
|
||||
input_stream = fibre.protocol.PacketFromStreamConverter(serial_device)
|
||||
output_stream = fibre.protocol.StreamBasedPacketSink(serial_device)
|
||||
channel = fibre.protocol.Channel(
|
||||
"serial port {}@{}".format(port_name, DEFAULT_BAUDRATE),
|
||||
input_stream, output_stream, channel_termination_token, logger)
|
||||
channel.serial_device = serial_device
|
||||
except serial.serialutil.SerialException:
|
||||
logger.debug("Serial device init failed. Ignoring this port. More info: " + traceback.format_exc())
|
||||
known_devices.append(port_name)
|
||||
else:
|
||||
known_devices.append(port_name)
|
||||
channel._channel_broken.subscribe(lambda: did_disconnect(port_name, serial_device))
|
||||
callback(channel)
|
||||
time.sleep(1)
|
||||
|
||||
@@ -0,0 +1,111 @@
|
||||
|
||||
import sys
|
||||
import platform
|
||||
import threading
|
||||
import fibre
|
||||
|
||||
interactive_variables = {}
|
||||
discovered_devices = []
|
||||
|
||||
def did_discover_device(device, branding_short, branding_long, logger, app_shutdown_token):
|
||||
"""
|
||||
Handles the discovery of new devices by displaying a
|
||||
message and making the device available to the interactive
|
||||
console
|
||||
"""
|
||||
serial_number = '{:012X}'.format(device.serial_number) if hasattr(device, 'serial_number') else "[unknown serial number]"
|
||||
if serial_number in discovered_devices:
|
||||
verb = "Reconnected"
|
||||
index = discovered_devices.index(serial_number)
|
||||
else:
|
||||
verb = "Connected"
|
||||
discovered_devices.append(serial_number)
|
||||
index = len(discovered_devices) - 1
|
||||
interactive_name = branding_short + str(index)
|
||||
|
||||
# Publish new device to interactive console
|
||||
interactive_variables[interactive_name] = device
|
||||
globals()[interactive_name] = device # Add to globals so tab complete works
|
||||
logger.info("{} to {} {} as {}".format(verb, branding_long, serial_number, interactive_name))
|
||||
|
||||
# Subscribe to disappearance of the device
|
||||
device.__channel__._channel_broken.subscribe(lambda: did_lose_device(interactive_name, logger, app_shutdown_token))
|
||||
|
||||
def did_lose_device(interactive_name, logger, app_shutdown_token):
|
||||
"""
|
||||
Handles the disappearance of a device by displaying
|
||||
a message.
|
||||
"""
|
||||
if not app_shutdown_token.is_set():
|
||||
logger.warn("Oh no {} disappeared".format(interactive_name))
|
||||
|
||||
def launch_shell(args,
|
||||
print_banner, print_help,
|
||||
logger, app_shutdown_token,
|
||||
branding_short="dev", branding_long="device"):
|
||||
"""
|
||||
Launches an interactive python or IPython command line
|
||||
interface.
|
||||
As devices are connected they are made available as
|
||||
"dev0", "dev1", ...
|
||||
The names of the variables can be customized by setting branding_short.
|
||||
"""
|
||||
|
||||
# Connect to device
|
||||
logger.debug("Waiting for {}...".format(branding_long))
|
||||
fibre.find_all(args.path, args.serial_number,
|
||||
lambda dev: did_discover_device(dev, branding_short, branding_long, logger, app_shutdown_token),
|
||||
app_shutdown_token,
|
||||
app_shutdown_token,
|
||||
logger=logger)
|
||||
|
||||
# Check if IPython is installed
|
||||
if args.no_ipython:
|
||||
use_ipython = False
|
||||
else:
|
||||
try:
|
||||
import IPython
|
||||
use_ipython = True
|
||||
except:
|
||||
print("Warning: you don't have IPython installed.")
|
||||
print("If you want to have an improved interactive console with pretty colors,")
|
||||
print("you should install IPython\n")
|
||||
use_ipython = False
|
||||
|
||||
interactive_variables["help"] = lambda: print_help(args, len(discovered_devices) > 0)
|
||||
|
||||
# If IPython is installed, embed IPython shell, otherwise embed regular shell
|
||||
if use_ipython:
|
||||
help = lambda: print_help(args, len(discovered_devices) > 0) # Override help function # pylint: disable=W0612
|
||||
console = IPython.terminal.embed.InteractiveShellEmbed(banner1='')
|
||||
console.runcode = console.run_code # hack to make IPython look like the regular console
|
||||
interact = console
|
||||
else:
|
||||
# Enable tab complete if possible
|
||||
try:
|
||||
import readline # Works only on Unix
|
||||
readline.parse_and_bind("tab: complete")
|
||||
except:
|
||||
sudo_prefix = "" if platform.system() == "Windows" else "sudo "
|
||||
print("Warning: could not enable tab-complete. User experience will suffer.\n"
|
||||
"Run `{}pip install readline` and then restart this script to fix this."
|
||||
.format(sudo_prefix))
|
||||
|
||||
import code
|
||||
console = code.InteractiveConsole(locals=interactive_variables)
|
||||
interact = lambda: console.interact(banner='')
|
||||
|
||||
# install hook to hide ChannelBrokenException
|
||||
console.runcode('import sys')
|
||||
console.runcode('superexcepthook = sys.excepthook')
|
||||
console.runcode('def newexcepthook(ex_class,ex,trace):\n'
|
||||
' if ex_class.__module__ + "." + ex_class.__name__ != "fibre.ChannelBrokenException":\n'
|
||||
' superexcepthook(ex_class,ex,trace)')
|
||||
console.runcode('sys.excepthook=newexcepthook')
|
||||
|
||||
|
||||
# Launch shell
|
||||
print_banner()
|
||||
logger._skip_bottom_line = True
|
||||
interact()
|
||||
app_shutdown_token.set()
|
||||
@@ -2,14 +2,15 @@
|
||||
import sys
|
||||
import socket
|
||||
import time
|
||||
import traceback
|
||||
import fibre.protocol
|
||||
from fibre.core import object_from_channel
|
||||
from fibre.utils import wait_any
|
||||
|
||||
def noprint(x):
|
||||
pass
|
||||
|
||||
class TCPTransport(fibre.protocol.StreamSource, fibre.protocol.StreamSink):
|
||||
def __init__(self, dest_addr, dest_port, printer):
|
||||
def __init__(self, dest_addr, dest_port, logger):
|
||||
# TODO: FIXME: use IPv6
|
||||
# Problem: getaddrinfo fails if the resolver returns an
|
||||
# IPv4 address, but we are using AF_INET6
|
||||
@@ -37,38 +38,48 @@ class TCPTransport(fibre.protocol.StreamSource, fibre.protocol.StreamSink):
|
||||
try:
|
||||
data = self.sock.recv(n_bytes, socket.MSG_WAITALL) # receive n_bytes
|
||||
return data
|
||||
except TimeoutError:
|
||||
except socket.timeout:
|
||||
# if we got a timeout data will still be none, so we call recv again
|
||||
# this time in non blocking state and see if we can get some data
|
||||
return self.sock.recv(n_bytes, socket.MSG_DONTWAIT)
|
||||
try:
|
||||
return self.sock.recv(n_bytes, socket.MSG_DONTWAIT)
|
||||
except socket.timeout:
|
||||
raise TimeoutError
|
||||
|
||||
def get_bytes_or_fail(self, n_bytes, deadline):
|
||||
result = self.get_bytes(n_bytes, deadline)
|
||||
if len(result) < n_bytes:
|
||||
raise fibre.protocol.TimeoutException("expected {} bytes but got only {}".format(n_bytes, len(result)))
|
||||
raise TimeoutError("expected {} bytes but got only {}".format(n_bytes, len(result)))
|
||||
return result
|
||||
|
||||
|
||||
|
||||
def channel_from_tcp_destination(dest_addr, dest_port, printer=noprint, device_stdout=noprint):
|
||||
"""
|
||||
Inits a Fibre Protocol channel from a TCP hostname and port.
|
||||
"""
|
||||
tcp_transport = fibre.tcp_transport.TCPTransport(dest_addr, dest_port, printer)
|
||||
stream2packet_input = fibre.protocol.PacketFromStreamConverter(tcp_transport, device_stdout)
|
||||
packet2stream_output = fibre.protocol.StreamBasedPacketSink(tcp_transport)
|
||||
return fibre.protocol.Channel(
|
||||
"TCP device {}:{}".format(dest_addr, dest_port),
|
||||
stream2packet_input, packet2stream_output,
|
||||
device_stdout)
|
||||
|
||||
def open_tcp(destination, printer=noprint, device_stdout=noprint):
|
||||
def discover_channels(path, serial_number, callback, cancellation_token, channel_termination_token, logger):
|
||||
"""
|
||||
Tries to connect to a TCP server based on the path spec.
|
||||
This function blocks until cancellation_token is set.
|
||||
Channels spawned by this function run until channel_termination_token is set.
|
||||
"""
|
||||
try:
|
||||
dest_addr = ':'.join(destination.split(":")[:-1])
|
||||
dest_port = int(destination.split(":")[-1])
|
||||
dest_addr = ':'.join(path.split(":")[:-1])
|
||||
dest_port = int(path.split(":")[-1])
|
||||
except (ValueError, IndexError):
|
||||
raise Exception('"{}" is not a valid TCP destination. The format should be something like "localhost:1234".'
|
||||
.format(destination))
|
||||
channel = channel_from_tcp_destination(dest_addr, dest_port)
|
||||
tcp_device = object_from_channel(channel, printer)
|
||||
return tcp_device
|
||||
.format(path))
|
||||
|
||||
while not cancellation_token.is_set():
|
||||
try:
|
||||
tcp_transport = fibre.tcp_transport.TCPTransport(dest_addr, dest_port, logger)
|
||||
stream2packet_input = fibre.protocol.PacketFromStreamConverter(tcp_transport)
|
||||
packet2stream_output = fibre.protocol.StreamBasedPacketSink(tcp_transport)
|
||||
channel = fibre.protocol.Channel(
|
||||
"TCP device {}:{}".format(dest_addr, dest_port),
|
||||
stream2packet_input, packet2stream_output,
|
||||
channel_termination_token, logger)
|
||||
except:
|
||||
#logger.debug("TCP channel init failed. More info: " + traceback.format_exc())
|
||||
pass
|
||||
else:
|
||||
callback(channel)
|
||||
wait_any(None, cancellation_token, channel._channel_broken)
|
||||
time.sleep(1)
|
||||
|
||||
@@ -1,14 +1,16 @@
|
||||
|
||||
import sys
|
||||
import socket
|
||||
import time
|
||||
import traceback
|
||||
import fibre.protocol
|
||||
from fibre.core import object_from_channel
|
||||
from fibre.utils import wait_any
|
||||
|
||||
def noprint(x):
|
||||
pass
|
||||
|
||||
class UDPTransport(fibre.protocol.PacketSource, fibre.protocol.PacketSink):
|
||||
def __init__(self, dest_addr, dest_port, printer):
|
||||
def __init__(self, dest_addr, dest_port, logger):
|
||||
# TODO: FIXME: use IPv6
|
||||
# Problem: getaddrinfo fails if the resolver returns an
|
||||
# IPv4 address, but we are using AF_INET6
|
||||
@@ -26,25 +28,30 @@ class UDPTransport(fibre.protocol.PacketSource, fibre.protocol.PacketSink):
|
||||
data, addr = self.sock.recvfrom(1024)
|
||||
return data
|
||||
|
||||
|
||||
|
||||
def channel_from_udp_destination(dest_addr, dest_port, printer=noprint, device_stdout=noprint):
|
||||
"""
|
||||
Inits a Fibre Protocol channel from a UDP hostname and port.
|
||||
"""
|
||||
udp_transport = fibre.udp_transport.UDPTransport(dest_addr, dest_port, printer)
|
||||
return fibre.protocol.Channel(
|
||||
"UDP device {}:{}".format(dest_addr, dest_port),
|
||||
udp_transport, udp_transport,
|
||||
device_stdout)
|
||||
|
||||
def open_udp(destination, printer=noprint, device_stdout=noprint):
|
||||
def discover_channels(path, serial_number, callback, cancellation_token, channel_termination_token, logger):
|
||||
"""
|
||||
Tries to connect to a UDP server based on the path spec.
|
||||
This function blocks until cancellation_token is set.
|
||||
Channels spawned by this function run until channel_termination_token is set.
|
||||
"""
|
||||
try:
|
||||
dest_addr = ':'.join(destination.split(":")[:-1])
|
||||
dest_port = int(destination.split(":")[-1])
|
||||
dest_addr = ':'.join(path.split(":")[:-1])
|
||||
dest_port = int(path.split(":")[-1])
|
||||
except (ValueError, IndexError):
|
||||
raise Exception('"{}" is not a valid UDP destination. The format should be something like "localhost:1234".'
|
||||
.format(destination))
|
||||
channel = channel_from_udp_destination(dest_addr, dest_port)
|
||||
udp_device = object_from_channel(channel, printer)
|
||||
return udp_device
|
||||
.format(path))
|
||||
|
||||
while not cancellation_token.is_set():
|
||||
try:
|
||||
udp_transport = fibre.udp_transport.UDPTransport(dest_addr, dest_port, logger)
|
||||
channel = fibre.protocol.Channel(
|
||||
"UDP device {}:{}".format(dest_addr, dest_port),
|
||||
udp_transport, udp_transport,
|
||||
channel_termination_token, logger)
|
||||
except:
|
||||
logger.debug("UDP channel init failed. More info: " + traceback.format_exc())
|
||||
pass
|
||||
else:
|
||||
callback(channel)
|
||||
wait_any(None, cancellation_token, channel._channel_broken)
|
||||
time.sleep(1)
|
||||
|
||||
@@ -6,21 +6,24 @@ import usb.util
|
||||
import sys
|
||||
import time
|
||||
import fibre.protocol
|
||||
import traceback
|
||||
import platform
|
||||
|
||||
# Currently we identify fibre-enabled devices by VID,PID
|
||||
# TODO: identify by USB descriptors
|
||||
WELL_KNOWN_VID_PID_PAIRS = [
|
||||
(0x1209, 0x0D31),
|
||||
(0x1209, 0x0D32),
|
||||
(0x1209, 0x0D33)
|
||||
]
|
||||
|
||||
def noprint(x):
|
||||
pass
|
||||
|
||||
class USBBulkTransport(fibre.protocol.PacketSource, fibre.protocol.PacketSink):
|
||||
def __init__(self, dev, printer=noprint):
|
||||
def __init__(self, dev, logger):
|
||||
self._logger = logger
|
||||
self.dev = dev
|
||||
self.intf = None
|
||||
self._name = "USB device {}:{}".format(dev.idVendor, dev.idProduct)
|
||||
self._was_damaged = False
|
||||
|
||||
##
|
||||
# information about the connected device
|
||||
@@ -31,25 +34,31 @@ class USBBulkTransport(fibre.protocol.PacketSource, fibre.protocol.PacketSink):
|
||||
for cfg in self.dev:
|
||||
string += "ConfigurationValue {0}\n".format(cfg.bConfigurationValue)
|
||||
for intf in cfg:
|
||||
string += "\tInterfaceNumber {0},{0}\n".format(intf.bInterfaceNumber, intf.bAlternateSetting)
|
||||
string += "\tInterfaceNumber {0},{1}\n".format(intf.bInterfaceNumber, intf.bAlternateSetting)
|
||||
for ep in intf:
|
||||
string += "\t\tEndpointAddress {0}\n".format(ep.bEndpointAddress)
|
||||
return string
|
||||
|
||||
def init(self, printer=noprint):
|
||||
# detach kernel driver
|
||||
def init(self):
|
||||
# Under some conditions, the Linux USB/libusb stack ends up in a corrupt
|
||||
# state where there are a few packets in a receive queue but a call
|
||||
# to epr.read() does not return these packet until a new packet arrives.
|
||||
# This undesirable queue can be cleared by resetting the device.
|
||||
# On windows this would cause file-not-found errors in subsequent dev calls
|
||||
if platform.system() != 'Windows':
|
||||
self.dev.reset()
|
||||
|
||||
interface_number = 1
|
||||
try:
|
||||
if self.dev.is_kernel_driver_active(1):
|
||||
self.dev.detach_kernel_driver(1)
|
||||
printer("Detached Kernel Driver\n")
|
||||
if self.dev.is_kernel_driver_active(interface_number):
|
||||
self.dev.detach_kernel_driver(interface_number)
|
||||
self._logger.debug("Detached Kernel Driver")
|
||||
except NotImplementedError:
|
||||
pass #is_kernel_driver_active not implemented on Windows
|
||||
# set the active configuration. With no arguments, the first
|
||||
# configuration will be the active one
|
||||
self.dev.set_configuration()
|
||||
# get an endpoint instance
|
||||
|
||||
self.dev.set_configuration() # no args: set first configuration
|
||||
self.cfg = self.dev.get_active_configuration()
|
||||
self.intf = self.cfg[(1,0)]
|
||||
self.intf = self.cfg[(1,0)] # this implicitly claims the interface
|
||||
# write endpoint
|
||||
self.epw = usb.util.find_descriptor(self.intf,
|
||||
# match the first OUT endpoint
|
||||
@@ -59,7 +68,7 @@ class USBBulkTransport(fibre.protocol.PacketSource, fibre.protocol.PacketSink):
|
||||
usb.util.ENDPOINT_OUT
|
||||
)
|
||||
assert self.epw is not None
|
||||
printer("EndpointAddress for writing {}\n".format(self.epw.bEndpointAddress))
|
||||
self._logger.debug("EndpointAddress for writing {}".format(self.epw.bEndpointAddress))
|
||||
# read endpoint
|
||||
self.epr = usb.util.find_descriptor(self.intf,
|
||||
# match the first IN endpoint
|
||||
@@ -69,72 +78,122 @@ class USBBulkTransport(fibre.protocol.PacketSource, fibre.protocol.PacketSink):
|
||||
usb.util.ENDPOINT_IN
|
||||
)
|
||||
assert self.epr is not None
|
||||
printer("EndpointAddress for reading {}\n".format(self.epr.bEndpointAddress))
|
||||
self._logger.debug("EndpointAddress for reading {}".format(self.epr.bEndpointAddress))
|
||||
|
||||
def shutdown(self):
|
||||
return 0
|
||||
def deinit(self):
|
||||
if not self.intf is None:
|
||||
usb.util.release_interface(self.dev, self.intf)
|
||||
|
||||
def process_packet(self, usbBuffer):
|
||||
try:
|
||||
ret = self.epw.write(usbBuffer, 0)
|
||||
if self._was_damaged:
|
||||
self._logger.debug("Recovered from USB halt/stall condition")
|
||||
self._was_damaged = False
|
||||
return ret
|
||||
except usb.core.USBError as ex:
|
||||
if ex.errno == 19: # "no such device"
|
||||
if ex.errno == 19 or ex.errno == 32: # "no such device", "pipe error"
|
||||
raise fibre.protocol.ChannelBrokenException()
|
||||
elif ex.errno == 110: # timeout
|
||||
raise TimeoutError()
|
||||
else:
|
||||
raise
|
||||
self._logger.debug(traceback.format_exc())
|
||||
self._logger.debug("halt condition: {}".format(ex.errno))
|
||||
# Try resetting halt/stall condition
|
||||
try:
|
||||
self.deinit()
|
||||
self.init()
|
||||
except usb.core.USBError:
|
||||
raise fibre.protocol.ChannelBrokenException()
|
||||
# Retry transfer
|
||||
self._was_damaged = True
|
||||
raise fibre.protocol.ChannelDamagedException()
|
||||
|
||||
def get_packet(self, deadline):
|
||||
try:
|
||||
bufferLen = self.epr.wMaxPacketSize
|
||||
timeout = max(int((deadline - time.monotonic()) * 1000), 0)
|
||||
ret = self.epr.read(bufferLen, timeout)
|
||||
return ret
|
||||
if self._was_damaged:
|
||||
self._logger.debug("Recovered from USB halt/stall condition")
|
||||
self._was_damaged = False
|
||||
return bytearray(ret)
|
||||
except usb.core.USBError as ex:
|
||||
if ex.errno == 19: # "no such device"
|
||||
if ex.errno == 19 or ex.errno == 32: # "no such device", "pipe error"
|
||||
raise fibre.protocol.ChannelBrokenException()
|
||||
elif ex.errno is None or ex.errno == 110: # timeout
|
||||
raise TimeoutError()
|
||||
else:
|
||||
raise
|
||||
|
||||
def send_max(self):
|
||||
return 64
|
||||
|
||||
def receive_max(self):
|
||||
return 64
|
||||
self._logger.debug(traceback.format_exc())
|
||||
self._logger.debug("halt condition: {}".format(ex.errno))
|
||||
# Try resetting halt/stall condition
|
||||
try:
|
||||
self.deinit()
|
||||
self.init()
|
||||
except usb.core.USBError:
|
||||
raise fibre.protocol.ChannelBrokenException()
|
||||
# Retry transfer
|
||||
self._was_damaged = True
|
||||
raise fibre.protocol.ChannelDamagedException()
|
||||
|
||||
|
||||
def channel_from_usb_device(usb_device, printer=noprint, device_stdout=noprint):
|
||||
"""
|
||||
Inits a Fibre Protocol channel from a PyUSB device object.
|
||||
"""
|
||||
bulk_device = fibre.usbbulk_transport.USBBulkTransport(usb_device, printer)
|
||||
printer(bulk_device.info())
|
||||
bulk_device.init(printer)
|
||||
return fibre.protocol.Channel(
|
||||
"USB device bus {} device {}".format(usb_device.bus, usb_device.address),
|
||||
bulk_device, bulk_device,
|
||||
device_stdout)
|
||||
def discover_channels(path, serial_number, callback, cancellation_token, channel_termination_token, logger):
|
||||
"""
|
||||
Scans for USB devices that match the path spec.
|
||||
This function blocks until cancellation_token is set.
|
||||
Channels spawned by this function run until channel_termination_token is set.
|
||||
"""
|
||||
if path == None or path == "":
|
||||
bus = None
|
||||
address = None
|
||||
else:
|
||||
try:
|
||||
bus = int(path.split(":")[0])
|
||||
address = int(path.split(":")[1])
|
||||
except (ValueError, IndexError):
|
||||
raise Exception("{} is not a valid USB path specification. "
|
||||
"Expected a string of the format BUS:DEVICE where BUS "
|
||||
"and DEVICE are integers.".format(path))
|
||||
|
||||
known_devices = []
|
||||
def device_matcher(device):
|
||||
#print(" test {:04X}:{:04X}".format(device.idVendor, device.idProduct))
|
||||
if (device.bus, device.address) in known_devices:
|
||||
return False
|
||||
if bus != None and device.bus != bus:
|
||||
return False
|
||||
if address != None and device.address != address:
|
||||
return False
|
||||
if serial_number != None and device.serial_number != serial_number:
|
||||
return False
|
||||
if (device.idVendor, device.idProduct) not in WELL_KNOWN_VID_PID_PAIRS:
|
||||
return False
|
||||
return True
|
||||
|
||||
def find_usb_channels(vid_pid_pairs=WELL_KNOWN_VID_PID_PAIRS, printer=noprint, device_stdout=noprint):
|
||||
"""
|
||||
Scans for compatible USB devices.
|
||||
Returns a generator of fibre.protocol.Channel objects.
|
||||
"""
|
||||
for vid_pid_pair in vid_pid_pairs:
|
||||
for usb_device in usb.core.find(idVendor=vid_pid_pair[0], idProduct=vid_pid_pair[1], find_all=True):
|
||||
printer("Found Fibre Hub via PyUSB")
|
||||
try:
|
||||
yield channel_from_usb_device(usb_device, printer, device_stdout)
|
||||
except usb.core.USBError as ex:
|
||||
if ex.errno == 13:
|
||||
printer("USB device access denied. Did you set up your udev rules correctly?")
|
||||
continue
|
||||
raise
|
||||
|
||||
def open_usb(bus, address, printer=noprint, device_stdout=noprint):
|
||||
usb_device1 = usb.core.find(bus=1, address=16)
|
||||
usb_device = usb.core.find(bus=bus, address=address)
|
||||
if usb_device is None:
|
||||
raise fibre.protocol.DeviceInitException("No USB device found on bus {} device {}".format(bus, address))
|
||||
channel = channel_from_usb_device(usb_device, printer, device_stdout)
|
||||
return object_from_channel(channel, printer)
|
||||
while not cancellation_token.is_set():
|
||||
# logger.debug("USB discover loop")
|
||||
devices = usb.core.find(find_all=True, custom_match=device_matcher)
|
||||
for usb_device in devices:
|
||||
try:
|
||||
bulk_device = USBBulkTransport(usb_device, logger)
|
||||
logger.debug(bulk_device.info())
|
||||
bulk_device.init()
|
||||
channel = fibre.protocol.Channel(
|
||||
"USB device bus {} device {}".format(usb_device.bus, usb_device.address),
|
||||
bulk_device, bulk_device, channel_termination_token, logger)
|
||||
channel.usb_device = usb_device # for debugging only
|
||||
except usb.core.USBError as ex:
|
||||
if ex.errno == 13:
|
||||
logger.debug("USB device access denied. Did you set up your udev rules correctly?")
|
||||
continue
|
||||
elif ex.errno == 16:
|
||||
logger.debug("USB device busy. I'll reset it and try again.")
|
||||
usb_device.reset()
|
||||
continue
|
||||
else:
|
||||
logger.debug("USB device init failed. Ignoring this device. More info: " + traceback.format_exc())
|
||||
known_devices.append((usb_device.bus, usb_device.address))
|
||||
else:
|
||||
known_devices.append((usb_device.bus, usb_device.address))
|
||||
callback(channel)
|
||||
time.sleep(1)
|
||||
|
||||
@@ -0,0 +1,221 @@
|
||||
|
||||
import sys
|
||||
import time
|
||||
import threading
|
||||
import platform
|
||||
import subprocess
|
||||
import os
|
||||
|
||||
try:
|
||||
if platform.system() == 'Windows':
|
||||
import win32console
|
||||
# TODO: we should win32console anyway so we could just omit colorama
|
||||
import colorama
|
||||
colorama.init()
|
||||
except ModuleNotFoundError:
|
||||
print("Could not init terminal features.")
|
||||
sys.stdout.flush()
|
||||
pass
|
||||
|
||||
## Threading utils ##
|
||||
|
||||
class Event():
|
||||
"""
|
||||
Alternative to threading.Event(), enhanced by the subscribe() function
|
||||
that the original fails to provide.
|
||||
@param Trigger: if supplied, the newly created event will be triggered
|
||||
as soon as the trigger event becomes set
|
||||
"""
|
||||
def __init__(self, trigger=None):
|
||||
self._evt = threading.Event()
|
||||
self._subscribers = []
|
||||
self._mutex = threading.Lock()
|
||||
if not trigger is None:
|
||||
trigger.subscribe(lambda: self.set())
|
||||
|
||||
def is_set(self):
|
||||
return self._evt.is_set()
|
||||
|
||||
def set(self):
|
||||
"""
|
||||
Sets the event and invokes all subscribers if the event was
|
||||
not already set
|
||||
"""
|
||||
self._mutex.acquire()
|
||||
try:
|
||||
if not self._evt.is_set():
|
||||
self._evt.set()
|
||||
for s in self._subscribers:
|
||||
s()
|
||||
finally:
|
||||
self._mutex.release()
|
||||
|
||||
def subscribe(self, handler):
|
||||
"""
|
||||
Invokes the specified handler exactly once as soon as the
|
||||
specified event is set. If the event is already set, the
|
||||
handler is invoked immediately.
|
||||
Returns a function that can be invoked to unsubscribe.
|
||||
"""
|
||||
if handler is None:
|
||||
raise TypeError
|
||||
self._mutex.acquire()
|
||||
try:
|
||||
self._subscribers.append(handler)
|
||||
if self._evt.is_set():
|
||||
handler()
|
||||
finally:
|
||||
self._mutex.release()
|
||||
return handler
|
||||
|
||||
def unsubscribe(self, handler):
|
||||
self._mutex.acquire()
|
||||
try:
|
||||
self._subscribers.pop(self._subscribers.index(handler))
|
||||
finally:
|
||||
self._mutex.release()
|
||||
|
||||
def wait(self, timeout=None):
|
||||
if not self._evt.wait(timeout=timeout):
|
||||
raise TimeoutError()
|
||||
|
||||
def trigger_after(self, timeout):
|
||||
"""
|
||||
Triggers the event after the specified timeout.
|
||||
This function returns immediately.
|
||||
"""
|
||||
def delayed_trigger():
|
||||
if not self.wait(timeout=timeout):
|
||||
self.set()
|
||||
threading.Thread(target=delayed_trigger, daemon=True).start()
|
||||
|
||||
def wait_any(timeout=None, *events):
|
||||
"""
|
||||
Blocks until any of the specified events are triggered.
|
||||
Returns the index of the event that was triggerd or raises
|
||||
a TimeoutError
|
||||
Param timeout: A timeout in seconds
|
||||
"""
|
||||
or_event = threading.Event()
|
||||
subscriptions = []
|
||||
for event in events:
|
||||
subscriptions.append((event, event.subscribe(lambda: or_event.set())))
|
||||
or_event.wait(timeout=timeout)
|
||||
for event, sub in subscriptions:
|
||||
event.unsubscribe(sub)
|
||||
for i in range(len(events)):
|
||||
if events[i].is_set():
|
||||
return i
|
||||
raise TimeoutError()
|
||||
|
||||
|
||||
## Log utils ##
|
||||
|
||||
class Logger():
|
||||
"""
|
||||
Logs messages to stdout
|
||||
"""
|
||||
|
||||
COLOR_DEFAULT = 0
|
||||
COLOR_GREEN = 1
|
||||
COLOR_CYAN = 2
|
||||
COLOR_YELLOW = 3
|
||||
COLOR_RED = 4
|
||||
|
||||
_VT100Colors = {
|
||||
COLOR_GREEN: '\x1b[92;1m',
|
||||
COLOR_CYAN: '\x1b[96;1m',
|
||||
COLOR_YELLOW: '\x1b[93;1m',
|
||||
COLOR_RED: '\x1b[91;1m',
|
||||
COLOR_DEFAULT: '\x1b[0m'
|
||||
}
|
||||
|
||||
_Win32Colors = {
|
||||
COLOR_GREEN: 0x0A,
|
||||
COLOR_CYAN: 0x0B,
|
||||
COLOR_YELLOW: 0x0E,
|
||||
COLOR_RED: 0x0C,
|
||||
COLOR_DEFAULT: 0x07
|
||||
}
|
||||
|
||||
def __init__(self, verbose=True):
|
||||
self._prefix = ''
|
||||
self._skip_bottom_line = False # If true, messages are printed one line above the cursor
|
||||
self._verbose = verbose
|
||||
self._print_lock = threading.Lock()
|
||||
if platform.system() == 'Windows':
|
||||
self._stdout_buf = win32console.GetStdHandle(win32console.STD_OUTPUT_HANDLE)
|
||||
|
||||
def indent(self, prefix=' '):
|
||||
indented_logger = Logger()
|
||||
indented_logger._prefix = self._prefix + prefix
|
||||
return indented_logger
|
||||
|
||||
def print_on_second_last_line(self, text, color):
|
||||
"""
|
||||
Prints a text on the second last line.
|
||||
This can be used to print a message above the command
|
||||
prompt. If the command prompt spans multiple lines
|
||||
there will be glitches.
|
||||
If the printed text spans multiple lines there will also
|
||||
be glitches (though this could be fixed).
|
||||
"""
|
||||
|
||||
if platform.system() == 'Windows':
|
||||
# Windows <10 doesn't understand VT100 escape codes and the colorama
|
||||
# also doesn't support the specific escape codes we need so we use the
|
||||
# native Win32 API.
|
||||
info = self._stdout_buf.GetConsoleScreenBufferInfo()
|
||||
cursor_pos = info['CursorPosition']
|
||||
scroll_rect=win32console.PySMALL_RECTType(
|
||||
Left=0, Top=1,
|
||||
Right=info['Window'].Right,
|
||||
Bottom=cursor_pos.Y-1)
|
||||
scroll_dest = win32console.PyCOORDType(scroll_rect.Left, scroll_rect.Top-1)
|
||||
self._stdout_buf.ScrollConsoleScreenBuffer(
|
||||
scroll_rect, scroll_rect, scroll_dest, # clipping rect is same as scroll rect
|
||||
u' ', Logger._Win32Colors[color]) # fill with empty cells with the desired color attributes
|
||||
line_start = win32console.PyCOORDType(0, cursor_pos.Y-1)
|
||||
self._stdout_buf.WriteConsoleOutputCharacter(text, line_start)
|
||||
|
||||
else:
|
||||
# Assume we're in a terminal that interprets VT100 escape codes.
|
||||
# TODO: test on macOS
|
||||
|
||||
# Escape character sequence:
|
||||
# ESC 7: store cursor position
|
||||
# ESC 1A: move cursor up by one
|
||||
# ESC 1S: scroll entire viewport by one
|
||||
# ESC 1L: insert 1 line at cursor position
|
||||
# (print text)
|
||||
# ESC 8: restore old cursor position
|
||||
|
||||
self._print_lock.acquire()
|
||||
sys.stdout.write('\x1b7\x1b[1A\x1b[1S\x1b[1L')
|
||||
sys.stdout.write(Logger._VT100Colors[color] + text + Logger._VT100Colors[Logger.COLOR_DEFAULT])
|
||||
sys.stdout.write('\x1b8')
|
||||
sys.stdout.flush()
|
||||
self._print_lock.release()
|
||||
|
||||
def print_colored(self, text, color):
|
||||
if self._skip_bottom_line:
|
||||
self.print_on_second_last_line(text, color)
|
||||
else:
|
||||
# On Windows, colorama does the job of interpreting the VT100 escape sequences
|
||||
self._print_lock.acquire()
|
||||
sys.stdout.write(Logger._VT100Colors[color] + text + Logger._VT100Colors[Logger.COLOR_DEFAULT] + '\n')
|
||||
sys.stdout.flush()
|
||||
self._print_lock.release()
|
||||
|
||||
def debug(self, text):
|
||||
if self._verbose:
|
||||
self.print_colored(self._prefix + text, Logger.COLOR_DEFAULT)
|
||||
def success(self, text):
|
||||
self.print_colored(self._prefix + text, Logger.COLOR_GREEN)
|
||||
def info(self, text):
|
||||
self.print_colored(self._prefix + text, Logger.COLOR_CYAN)
|
||||
def warn(self, text):
|
||||
self.print_colored(self._prefix + text, Logger.COLOR_YELLOW)
|
||||
def error(self, text):
|
||||
# TODO: write to stderr
|
||||
self.print_colored(self._prefix + text, Logger.COLOR_RED)
|
||||
@@ -1,87 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
Connect to a fibre-enabled device to play with in the IPython interactive shell.
|
||||
"""
|
||||
import argparse
|
||||
import sys
|
||||
import os
|
||||
|
||||
sys.path.insert(0, os.path.dirname(os.path.dirname(os.path.realpath(__file__))) + "/python")
|
||||
import fibre
|
||||
|
||||
# Parse arguments
|
||||
parser = argparse.ArgumentParser(description='Connect to a fibre-enabled device to play with it in the IPython interactive shell.')
|
||||
parser.add_argument("-v", "--verbose", action="store_true",
|
||||
help="print debug information")
|
||||
group = parser.add_mutually_exclusive_group()
|
||||
group.add_argument("-d", "--discover", metavar="CHANNELS", action="store",
|
||||
help="Automatically discover fibre-enabled devices. Takes a comma-separated list (without spaces) "
|
||||
"to indicate which connection types should be considered. Possible values are "
|
||||
"usb and serial. For example \"--discover usb,serial\" indicates "
|
||||
"that USB and serial ports should be scanned for fibre-enabled devices. "
|
||||
"If none of the below options are specified, --discover usb is assumed.")
|
||||
group.add_argument("-u", "--usb", metavar="BUS:DEVICE", action="store",
|
||||
help="Specifies the USB port on which the device is connected. "
|
||||
"For example \"001:014\" means bus 001, device 014. The numbers can be obtained "
|
||||
"using `lsusb`.")
|
||||
group.add_argument("-p", "--udp", metavar="ADDR:PORT", action="store",
|
||||
help="Specifies the UDP port on which the device is reachable. ")
|
||||
group.add_argument("-t", "--tcp", metavar="ADDR:PORT", action="store",
|
||||
help="Specifies the TCP port on which the device is reachable. ")
|
||||
group.add_argument("-s", "--serial", metavar="PORT", action="store",
|
||||
help="Specifies the serial port on which the device is connected. "
|
||||
"For example \"/dev/ttyUSB0\". Use `ls /dev/tty*` to find your port name.")
|
||||
parser.set_defaults(discover="usb")
|
||||
args = parser.parse_args()
|
||||
|
||||
if (args.verbose):
|
||||
printer = print
|
||||
else:
|
||||
printer = lambda x: None
|
||||
|
||||
|
||||
# Connect to device
|
||||
if not args.usb is None:
|
||||
try:
|
||||
bus = int(args.usb.split(":")[0])
|
||||
address = int(args.usb.split(":")[1])
|
||||
except (ValueError, IndexError):
|
||||
print("the --usb argument must look something like this: \"001:014\"")
|
||||
sys.exit(1)
|
||||
try:
|
||||
my_device = fibre.open_usb(bus, address, printer=printer, device_stdout=print)
|
||||
except fibre.protocol.DeviceInitException as ex:
|
||||
print(str(ex))
|
||||
sys.exit(1)
|
||||
elif not args.serial is None:
|
||||
my_device = fibre.open_serial(args.serial, printer=printer, device_stdout=print)
|
||||
elif not args.udp is None:
|
||||
my_device = fibre.open_udp(args.udp, printer=printer, device_stdout=print)
|
||||
elif not args.tcp is None:
|
||||
my_device = fibre.open_tcp(args.tcp, printer=printer, device_stdout=print)
|
||||
else:
|
||||
print("Waiting for device...")
|
||||
consider_usb = 'usb' in args.discover.split(',')
|
||||
consider_serial = 'serial' in args.discover.split(',')
|
||||
my_device = fibre.find_any(consider_usb, consider_serial, printer=printer, device_stdout=print)
|
||||
print("Connected!")
|
||||
|
||||
|
||||
try:
|
||||
# If this assignment works, we are already in interactive mode.
|
||||
# so just drop out of script to existing shell
|
||||
interpreter = sys.ps1
|
||||
except AttributeError:
|
||||
# We are not in interactive mode, so let's fire one up
|
||||
# Though let's be real, IPython is the way to go
|
||||
print('If you want to have an improved interactive console with pretty colors,')
|
||||
print('you can run this script in interactive mode with IPython with this command:')
|
||||
print('ipython -i explore_fibre.py')
|
||||
print('')
|
||||
# Enter interactive python shell with tab complete enabled
|
||||
import code
|
||||
import rlcompleter
|
||||
import readline
|
||||
readline.parse_and_bind("tab: complete")
|
||||
code.interact(local=locals(), banner='')
|
||||
|
||||
Executable
+57
@@ -0,0 +1,57 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
Connect to a Fibre-enabled device to play with in the IPython interactive shell.
|
||||
"""
|
||||
import argparse
|
||||
import sys
|
||||
import os
|
||||
|
||||
sys.path.insert(0, os.path.dirname(os.path.dirname(os.path.realpath(__file__))) + "/python")
|
||||
import fibre
|
||||
from fibre import Logger, Event
|
||||
|
||||
# Parse arguments
|
||||
parser = argparse.ArgumentParser(description='Connect to a fibre-enabled device to play with it in the IPython interactive shell.')
|
||||
parser.add_argument("-p", "--path", metavar="PATH", action="store",
|
||||
help="The path(s) where ODrive(s) should be discovered.\n"
|
||||
"By default the script will connect to any ODrive on USB.\n\n"
|
||||
"To select a specific USB device:\n"
|
||||
" --path usb:BUS:DEVICE\n"
|
||||
"usbwhere BUS and DEVICE are the bus and device numbers as shown in `lsusb`.\n\n"
|
||||
"To select a specific serial port:\n"
|
||||
" --path serial:PATH\n"
|
||||
"where PATH is the path of the serial port. For example \"/dev/ttyUSB0\".\n"
|
||||
"You can use `ls /dev/tty*` to find the correct port.\n\n"
|
||||
"You can combine USB and serial specs by separating them with a comma (no space!)\n"
|
||||
"Example:\n"
|
||||
" --path usb,serial:/dev/ttyUSB0\n"
|
||||
"means \"discover any USB device or a serial device on /dev/ttyUSB0\"")
|
||||
parser.add_argument("-s", "--serial-number", action="store",
|
||||
help="The 12-digit serial number of the device. "
|
||||
"This is a string consisting of 12 upper case hexadecimal "
|
||||
"digits as displayed in lsusb. \n"
|
||||
" example: 385F324D3037\n"
|
||||
"You can list all devices connected to USB by running\n"
|
||||
"(lsusb -d 1209:0d32 -v; lsusb -d 0483:df11 -v) | grep iSerial\n"
|
||||
"If omitted, any device is accepted.")
|
||||
parser.add_argument("--no-ipython", action="store_true",
|
||||
help="Use the regular Python shell "
|
||||
"instead of the IPython shell, "
|
||||
"even if IPython is installed.")
|
||||
parser.add_argument("-v", "--verbose", action="store_true",
|
||||
help="print debug information")
|
||||
|
||||
parser.set_defaults(path="usb,tcp:localhost:9910")
|
||||
args = parser.parse_args()
|
||||
|
||||
logger = Logger(verbose=args.verbose)
|
||||
app_shutdown_token = Event()
|
||||
|
||||
def print_banner():
|
||||
pass
|
||||
|
||||
def print_help(args, have_devices):
|
||||
pass
|
||||
|
||||
import fibre
|
||||
fibre.launch_shell(args, print_banner, print_help, logger, app_shutdown_token)
|
||||
@@ -3,3 +3,8 @@
|
||||
from .version import get_version_str
|
||||
__version__ = get_version_str()
|
||||
del get_version_str
|
||||
|
||||
|
||||
import fibre
|
||||
find_any = fibre.find_any
|
||||
find_all = fibre.find_all
|
||||
|
||||
+4
-3
@@ -12,7 +12,8 @@ import struct
|
||||
import array
|
||||
import fractions
|
||||
import usb.core
|
||||
import odrive.discovery
|
||||
import fibre
|
||||
import odrive
|
||||
from odrive.utils import Event
|
||||
from odrive.dfuse import *
|
||||
|
||||
@@ -222,7 +223,7 @@ def put_odrive_into_dfu_mode(my_drive, cancellation_token):
|
||||
print("Putting device {} into DFU mode...".format(my_drive.__channel__.usb_device.serial_number))
|
||||
try:
|
||||
my_drive.enter_dfu_mode()
|
||||
except odrive.protocol.ChannelBrokenException:
|
||||
except fibre.ChannelBrokenException:
|
||||
pass # this is expected because the device reboots
|
||||
if platform.system() == "Windows":
|
||||
show_deferred_message("Still waiting for the device to reappear.\n"
|
||||
@@ -259,7 +260,7 @@ def launch_dfu(args, app_shutdown_token):
|
||||
|
||||
# Scan for ODrives not in DFU mode and put them into DFU mode once they appear
|
||||
# We only scan on USB because DFU is only possible over USB
|
||||
odrive.discovery.find_all(args.path, serial_number,
|
||||
odrive.find_all(args.path, serial_number,
|
||||
lambda dev: put_odrive_into_dfu_mode(dev, find_odrive_cancellation_token),
|
||||
find_odrive_cancellation_token, app_shutdown_token)
|
||||
|
||||
|
||||
@@ -1,100 +0,0 @@
|
||||
"""
|
||||
Provides functions for the discovery of ODrive devices
|
||||
"""
|
||||
|
||||
import sys
|
||||
import json
|
||||
import time
|
||||
import threading
|
||||
import traceback
|
||||
import odrive.protocol
|
||||
import odrive.utils
|
||||
import odrive.remote_object
|
||||
import odrive.usbbulk_transport
|
||||
import odrive.serial_transport
|
||||
from odrive.utils import Event
|
||||
|
||||
channel_types = {
|
||||
"usb": odrive.usbbulk_transport.discover_channels,
|
||||
"serial": odrive.serial_transport.discover_channels
|
||||
}
|
||||
|
||||
def noprint(text):
|
||||
pass
|
||||
|
||||
def find_all(path, serial_number,
|
||||
did_discover_object_callback,
|
||||
search_cancellation_token,
|
||||
channel_termination_token, printer=noprint):
|
||||
"""
|
||||
Starts scanning for ODrives that match the specified path spec and calls
|
||||
the callback for each ODrive that is found.
|
||||
This function is non-blocking.
|
||||
"""
|
||||
|
||||
def did_discover_channel(channel):
|
||||
"""
|
||||
Inits an object from a given channel and then calls did_discover_object_callback
|
||||
with the created object
|
||||
This queries the endpoint 0 on that channel to gain information
|
||||
about the interface, which is then used to init the corresponding object.
|
||||
"""
|
||||
try:
|
||||
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)
|
||||
printer("JSON checksum: 0x{:02X} 0x{:02X}".format(json_crc16 & 0xff, (json_crc16 >> 8) & 0xff))
|
||||
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, printer)
|
||||
device_serial_number = format(obj.serial_number, 'x').upper() 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:
|
||||
printer("Unexpected exception after discovering channel: " + traceback.format_exc())
|
||||
|
||||
# For each connection type, kick off an appropriate discovery loop
|
||||
for search_spec in path.split(','):
|
||||
prefix = search_spec.split(':')[0]
|
||||
the_rest = ':'.join(search_spec.split(':')[1:])
|
||||
if prefix in channel_types:
|
||||
threading.Thread(target=channel_types[prefix],
|
||||
args=(the_rest, serial_number, did_discover_channel, search_cancellation_token, channel_termination_token, printer)).start()
|
||||
else:
|
||||
raise Exception("Invalid path spec \"{}\"".format(search_spec))
|
||||
|
||||
|
||||
def find_any(path="usb", serial_number=None,
|
||||
search_cancellation_token=None, channel_termination_token=None,
|
||||
timeout=None, printer=noprint):
|
||||
"""
|
||||
Blocks until the first matching ODrive is connected and then returns that device
|
||||
"""
|
||||
result = [ None ]
|
||||
done_signal = Event(search_cancellation_token)
|
||||
def did_discover_object(obj):
|
||||
result[0] = obj
|
||||
done_signal.set()
|
||||
find_all(path, serial_number, did_discover_object, done_signal, channel_termination_token, printer)
|
||||
try:
|
||||
done_signal.wait(timeout=timeout)
|
||||
finally:
|
||||
done_signal.set() # terminate find_all
|
||||
return result[0]
|
||||
@@ -1,356 +0,0 @@
|
||||
# See protocol.hpp for an overview of the protocol
|
||||
|
||||
import time
|
||||
import struct
|
||||
import sys
|
||||
import threading
|
||||
import traceback
|
||||
import odrive.utils
|
||||
from odrive.utils import wait_any
|
||||
from odrive.utils import Event
|
||||
|
||||
import abc
|
||||
|
||||
if sys.version_info >= (3, 4):
|
||||
ABC = abc.ABC
|
||||
else:
|
||||
ABC = abc.ABCMeta('ABC', (), {})
|
||||
|
||||
if sys.version_info < (3, 3):
|
||||
from monotonic import monotonic
|
||||
time.monotonic = monotonic
|
||||
|
||||
SYNC_BYTE = 0xAA
|
||||
CRC8_INIT = 0x42
|
||||
CRC16_INIT = 0x1337
|
||||
PROTOCOL_VERSION = 1
|
||||
|
||||
CRC8_DEFAULT = 0x37 # this must match the polynomial in the C++ implementation
|
||||
CRC16_DEFAULT = 0x3d65 # this must match the polynomial in the C++ implementation
|
||||
|
||||
MAX_PACKET_SIZE = 128
|
||||
|
||||
def calc_crc(remainder, value, polynomial, bitwidth):
|
||||
topbit = (1 << (bitwidth - 1))
|
||||
|
||||
# Bring the next byte into the remainder.
|
||||
remainder ^= (value << (bitwidth - 8))
|
||||
for bitnumber in range(0,8):
|
||||
if (remainder & topbit):
|
||||
remainder = (remainder << 1) ^ polynomial
|
||||
else:
|
||||
remainder = (remainder << 1)
|
||||
|
||||
return remainder & ((1 << bitwidth) - 1)
|
||||
|
||||
def calc_crc8(remainder, value):
|
||||
if isinstance(value, bytearray) or isinstance(value, bytes) or isinstance(value, list):
|
||||
for byte in value:
|
||||
if not isinstance(byte,int):
|
||||
byte = ord(byte)
|
||||
remainder = calc_crc(remainder, byte, CRC8_DEFAULT, 8)
|
||||
else:
|
||||
remainder = calc_crc(remainder, byte, CRC8_DEFAULT, 8)
|
||||
return remainder
|
||||
|
||||
def calc_crc16(remainder, value):
|
||||
if isinstance(value, bytearray) or isinstance(value, bytes) or isinstance(value, list):
|
||||
for byte in value:
|
||||
if not isinstance(byte, int):
|
||||
byte = ord(byte)
|
||||
remainder = calc_crc(remainder, byte, CRC16_DEFAULT, 16)
|
||||
else:
|
||||
remainder = calc_crc(remainder, value, CRC16_DEFAULT, 16)
|
||||
return remainder
|
||||
|
||||
# Can be verified with http://www.sunshine2k.de/coding/javascript/crc/crc_js.html:
|
||||
#print(hex(calc_crc8(0x12, [1, 2, 3, 4, 5, 0x10, 0x13, 0x37])))
|
||||
#print(hex(calc_crc16(0xfeef, [1, 2, 3, 4, 5, 0x10, 0x13, 0x37])))
|
||||
|
||||
|
||||
class DeviceInitException(Exception):
|
||||
pass
|
||||
|
||||
class ChannelDamagedException(Exception):
|
||||
"""
|
||||
Raised when the channel is temporarily broken and a
|
||||
resend of the message might be successful
|
||||
"""
|
||||
pass
|
||||
|
||||
class ChannelBrokenException(Exception):
|
||||
"""
|
||||
Raised when the channel is permanently broken
|
||||
"""
|
||||
pass
|
||||
|
||||
|
||||
class StreamSource(ABC):
|
||||
@abc.abstractmethod
|
||||
def get_bytes(self, deadline):
|
||||
pass
|
||||
|
||||
class StreamSink(ABC):
|
||||
@abc.abstractmethod
|
||||
def process_bytes(self, bytes):
|
||||
pass
|
||||
|
||||
class PacketSource(ABC):
|
||||
@abc.abstractmethod
|
||||
def get_packet(self, deadline):
|
||||
pass
|
||||
|
||||
class PacketSink(ABC):
|
||||
@abc.abstractmethod
|
||||
def process_packet(self, packet):
|
||||
pass
|
||||
|
||||
|
||||
class StreamToPacketConverter(StreamSink):
|
||||
def __init__(self, output):
|
||||
self._header = []
|
||||
self._packet = []
|
||||
self._packet_length = 0
|
||||
self._output = output
|
||||
|
||||
def process_bytes(self, bytes):
|
||||
"""
|
||||
Processes an arbitrary number of bytes. If one or more full packets are
|
||||
are received, they are sent to this instance's output PacketSink.
|
||||
Incomplete packets are buffered between subsequent calls to this function.
|
||||
"""
|
||||
|
||||
for byte in bytes:
|
||||
if (len(self._header) < 3):
|
||||
# Process header byte
|
||||
self._header.append(byte)
|
||||
if (len(self._header) == 1) and (self._header[0] != SYNC_BYTE):
|
||||
self._header = []
|
||||
elif (len(self._header) == 2) and (self._header[1] & 0x80):
|
||||
self._header = [] # TODO: support packets larger than 128 bytes
|
||||
elif (len(self._header) == 3) and calc_crc8(CRC8_INIT, self._header):
|
||||
self._header = []
|
||||
elif (len(self._header) == 3):
|
||||
self._packet_length = self._header[1] + 2
|
||||
else:
|
||||
# Process payload byte
|
||||
self._packet.append(byte)
|
||||
|
||||
# If both header and packet are fully received, hand it on to the packet processor
|
||||
if (len(self._header) == 3) and (len(self._packet) == self._packet_length):
|
||||
if calc_crc16(CRC16_INIT, self._packet) == 0:
|
||||
self._output.process_packet(self._packet[:-2])
|
||||
self._header = []
|
||||
self._packet = []
|
||||
self._packet_length = 0
|
||||
|
||||
|
||||
class PacketToStreamConverter(PacketSink):
|
||||
def __init__(self, output):
|
||||
self._output = output
|
||||
|
||||
def process_packet(self, packet):
|
||||
if (len(packet) >= MAX_PACKET_SIZE):
|
||||
raise NotImplementedError("packet larger than 127 currently not supported")
|
||||
|
||||
header = [SYNC_BYTE, len(packet)]
|
||||
header.append(calc_crc8(CRC8_INIT, header))
|
||||
|
||||
self._output.process_bytes(header)
|
||||
self._output.process_bytes(packet)
|
||||
|
||||
# append CRC in big endian
|
||||
crc16 = calc_crc16(CRC16_INIT, packet)
|
||||
self._output.process_bytes(struct.pack('>H', crc16))
|
||||
|
||||
class PacketFromStreamConverter(PacketSource):
|
||||
def __init__(self, input):
|
||||
self._input = input
|
||||
|
||||
def get_packet(self, deadline):
|
||||
"""
|
||||
Requests bytes from the underlying input stream until a full packet is
|
||||
received or the deadline is reached, in which case None is returned. A
|
||||
deadline before the current time corresponds to non-blocking mode.
|
||||
"""
|
||||
while True:
|
||||
header = bytes()
|
||||
|
||||
# TODO: sometimes this call hangs, even though the device apparently sent something
|
||||
header = header + self._input.get_bytes_or_fail(1, deadline)
|
||||
if (header[0] != SYNC_BYTE):
|
||||
#print("sync byte mismatch")
|
||||
continue
|
||||
|
||||
header = header + self._input.get_bytes_or_fail(1, deadline)
|
||||
if (header[1] & 0x80):
|
||||
#print("packet too large")
|
||||
continue # TODO: support packets larger than 128 bytes
|
||||
|
||||
header = header + self._input.get_bytes_or_fail(1, deadline)
|
||||
if calc_crc8(CRC8_INIT, header) != 0:
|
||||
#print("crc8 mismatch")
|
||||
continue
|
||||
|
||||
packet_length = header[1] + 2
|
||||
#print("wait for {} bytes".format(packet_length))
|
||||
packet = self._input.get_bytes_or_fail(packet_length, deadline)
|
||||
if calc_crc16(CRC16_INIT, packet) != 0:
|
||||
#print("crc16 mismatch")
|
||||
continue
|
||||
return packet[:-2]
|
||||
|
||||
|
||||
class Channel(PacketSink):
|
||||
# Choose these parameters to be sensible for a specific transport layer
|
||||
_resend_timeout = 0.1 # [s]
|
||||
_send_attempts = 5
|
||||
|
||||
def __init__(self, name, input, output, cancellation_token, printer):
|
||||
"""
|
||||
Params:
|
||||
input: A PacketSource where this channel will source packets from on
|
||||
demand. Alternatively packets can be provided to this channel
|
||||
directly by calling process_packet on this instance.
|
||||
output: A PacketSink where this channel will put outgoing packets.
|
||||
"""
|
||||
self._name = name
|
||||
self._input = input
|
||||
self._output = output
|
||||
self._printer = printer
|
||||
self._outbound_seq_no = 0
|
||||
self._interface_definition_crc = 0
|
||||
self._expected_acks = {}
|
||||
self._responses = {}
|
||||
self._my_lock = threading.Lock()
|
||||
self._channel_broken = Event(cancellation_token)
|
||||
self.start_receiver_thread(Event(self._channel_broken))
|
||||
|
||||
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():
|
||||
error_ctr = 0
|
||||
try:
|
||||
while (not cancellation_token.is_set() and not self._channel_broken.is_set()
|
||||
and error_ctr < 10):
|
||||
# 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:
|
||||
error_ctr += 1
|
||||
continue # try again
|
||||
if (error_ctr > 0):
|
||||
error_ctr -= 1
|
||||
# Process response
|
||||
# This should not throw an exception, otherwise the channel breaks
|
||||
self.process_packet(response)
|
||||
#print("receiver thread is exiting")
|
||||
except Exception:
|
||||
self._printer("receiver thread is exiting: " + traceback.format_exc())
|
||||
finally:
|
||||
self._channel_broken.set()
|
||||
threading.Thread(target=receiver_thread).start()
|
||||
|
||||
def remote_endpoint_operation(self, endpoint_id, input, expect_ack, output_length):
|
||||
if input is None:
|
||||
input = bytearray(0)
|
||||
if (len(input) >= 128):
|
||||
raise Exception("packet larger than 127 currently not supported")
|
||||
|
||||
if (expect_ack):
|
||||
endpoint_id |= 0x8000
|
||||
|
||||
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 ascii protocol
|
||||
packet = struct.pack('<HHH', seq_no, endpoint_id, output_length)
|
||||
packet = packet + input
|
||||
|
||||
crc16 = calc_crc16(CRC16_INIT, packet)
|
||||
if (endpoint_id & 0x7fff == 0):
|
||||
trailer = PROTOCOL_VERSION
|
||||
else:
|
||||
trailer = self._interface_definition_crc
|
||||
#print("append trailer " + trailer)
|
||||
packet = packet + struct.pack('<H', trailer)
|
||||
|
||||
if (expect_ack):
|
||||
ack_event = Event()
|
||||
self._expected_acks[seq_no] = ack_event
|
||||
try:
|
||||
attempt = 0
|
||||
while (attempt < self._send_attempts):
|
||||
self._my_lock.acquire()
|
||||
try:
|
||||
self._output.process_packet(packet)
|
||||
except ChannelDamagedException:
|
||||
attempt += 1
|
||||
continue # resend
|
||||
finally:
|
||||
self._my_lock.release()
|
||||
# Wait for ACK until the resend timeout is exceeded
|
||||
try:
|
||||
if wait_any(self._resend_timeout, ack_event, self._channel_broken) != 0:
|
||||
raise ChannelBrokenException()
|
||||
except odrive.utils.TimeoutException:
|
||||
attempt += 1
|
||||
continue # resend
|
||||
return self._responses.pop(seq_no)
|
||||
# TODO: record channel statistics
|
||||
raise ChannelBrokenException() # Too many resend attempts
|
||||
finally:
|
||||
self._expected_acks.pop(seq_no)
|
||||
self._responses.pop(seq_no, None)
|
||||
else:
|
||||
# fire and forget
|
||||
self._output.process_packet(packet)
|
||||
return None
|
||||
|
||||
def remote_endpoint_read_buffer(self, endpoint_id):
|
||||
"""
|
||||
Handles reads from long endpoints
|
||||
"""
|
||||
# TODO: handle device that could (maliciously) send infinite stream
|
||||
buffer = bytes()
|
||||
while True:
|
||||
chunk_length = 512
|
||||
chunk = self.remote_endpoint_operation(endpoint_id, struct.pack("<I", len(buffer)), True, chunk_length)
|
||||
if (len(chunk) == 0):
|
||||
break
|
||||
buffer += chunk
|
||||
return buffer
|
||||
|
||||
def process_packet(self, packet):
|
||||
#print("process packet")
|
||||
packet = bytes(packet)
|
||||
if (len(packet) < 2):
|
||||
raise Exception("packet too short")
|
||||
|
||||
seq_no = struct.unpack('<H', packet[0:2])[0]
|
||||
|
||||
if (seq_no & 0x8000):
|
||||
seq_no &= 0x7fff
|
||||
ack_signal = self._expected_acks.get(seq_no, None)
|
||||
if (ack_signal):
|
||||
self._responses[seq_no] = packet[2:]
|
||||
ack_signal.set()
|
||||
#print("received ack for packet " + str(seq_no))
|
||||
else:
|
||||
print("received unexpected ACK: " + str(seq_no))
|
||||
|
||||
else:
|
||||
#if (calc_crc16(CRC16_INIT, struct.pack('<HBB', PROTOCOL_VERSION, packet[-2], packet[-1]))):
|
||||
# raise Exception("CRC16 mismatch")
|
||||
print("endpoint requested")
|
||||
# TODO: handle local endpoint operation
|
||||
@@ -1,100 +0,0 @@
|
||||
"""
|
||||
Provides classes that implement the StreamSource/StreamSink and
|
||||
PacketSource/PacketSink interfaces for serial ports.
|
||||
"""
|
||||
|
||||
import os
|
||||
import re
|
||||
import time
|
||||
import traceback
|
||||
import serial
|
||||
import serial.tools.list_ports
|
||||
import odrive.protocol
|
||||
import odrive.utils
|
||||
|
||||
ODRIVE_BAUDRATE = 115200
|
||||
|
||||
class SerialStreamTransport(odrive.protocol.StreamSource, odrive.protocol.StreamSink):
|
||||
def __init__(self, port, baud):
|
||||
self._dev = serial.Serial(port, baud, timeout=1)
|
||||
|
||||
def process_bytes(self, bytes):
|
||||
self._dev.write(bytes)
|
||||
|
||||
def get_bytes(self, n_bytes, deadline):
|
||||
"""
|
||||
Returns n bytes unless the deadline is reached, in which case the bytes
|
||||
that were read up to that point are returned. If deadline is None the
|
||||
function blocks forever. A deadline before the current time corresponds
|
||||
to non-blocking mode.
|
||||
"""
|
||||
if deadline is None:
|
||||
self._dev.timeout = None
|
||||
else:
|
||||
self._dev.timeout = max(deadline - time.monotonic(), 0)
|
||||
return self._dev.read(n_bytes)
|
||||
|
||||
def get_bytes_or_fail(self, n_bytes, deadline):
|
||||
result = self.get_bytes(n_bytes, deadline)
|
||||
if len(result) < n_bytes:
|
||||
raise odrive.utils.TimeoutException("expected {} bytes but got only {}", n_bytes, len(result))
|
||||
return result
|
||||
|
||||
def close(self):
|
||||
self._dev.close()
|
||||
|
||||
|
||||
def find_dev_serial_ports():
|
||||
try:
|
||||
return ['/dev/' + x for x in os.listdir('/dev')]
|
||||
except FileNotFoundError:
|
||||
return []
|
||||
|
||||
def find_pyserial_ports():
|
||||
return [x.device for x in serial.tools.list_ports.comports()]
|
||||
|
||||
|
||||
def discover_channels(path, serial_number, callback, cancellation_token, channel_termination_token, printer):
|
||||
"""
|
||||
Scans for serial ports that match the path spec.
|
||||
This function blocks until cancellation_token is set.
|
||||
Channels spawned by this function run until channel_termination_token is set.
|
||||
"""
|
||||
if path == None:
|
||||
# This regex should match all desired port names on macOS,
|
||||
# Linux and Windows but might match some incorrect port names.
|
||||
regex = r'^(/dev/tty\.usbmodem.*|/dev/ttyACM.*|COM[0-9]+)$'
|
||||
else:
|
||||
regex = "^" + path + "$"
|
||||
|
||||
known_devices = []
|
||||
def device_matcher(port_name):
|
||||
if port_name in known_devices:
|
||||
return False
|
||||
return bool(re.match(regex, port_name))
|
||||
|
||||
def did_disconnect(port_name, device):
|
||||
device.close()
|
||||
# TODO: yes there is a race condition here in case you wonder.
|
||||
known_devices.pop(known_devices.index(port_name))
|
||||
|
||||
while not cancellation_token.is_set():
|
||||
all_ports = find_pyserial_ports() + find_dev_serial_ports()
|
||||
new_ports = filter(device_matcher, all_ports)
|
||||
for port_name in new_ports:
|
||||
try:
|
||||
serial_device = SerialStreamTransport(port_name, ODRIVE_BAUDRATE)
|
||||
input_stream = odrive.protocol.PacketFromStreamConverter(serial_device)
|
||||
output_stream = odrive.protocol.PacketToStreamConverter(serial_device)
|
||||
channel = odrive.protocol.Channel(
|
||||
"serial port {}@{}".format(port_name, ODRIVE_BAUDRATE),
|
||||
input_stream, output_stream, channel_termination_token, printer)
|
||||
channel.serial_device = serial_device
|
||||
except serial.serialutil.SerialException:
|
||||
printer("Serial device init failed. Ignoring this port. More info: " + traceback.format_exc())
|
||||
known_devices.append(port_name)
|
||||
else:
|
||||
known_devices.append(port_name)
|
||||
channel._channel_broken.subscribe(lambda: did_disconnect(port_name, serial_device))
|
||||
callback(channel)
|
||||
time.sleep(1)
|
||||
+43
-96
@@ -2,7 +2,8 @@
|
||||
import sys
|
||||
import platform
|
||||
import threading
|
||||
import odrive.discovery
|
||||
import fibre
|
||||
import odrive
|
||||
from odrive.utils import start_liveplotter
|
||||
from odrive.enums import * # pylint: disable=W0614
|
||||
|
||||
@@ -10,9 +11,9 @@ def print_banner():
|
||||
print('Please connect your ODrive.')
|
||||
print('You can also type help() or quit().')
|
||||
|
||||
def print_help(args):
|
||||
def print_help(args, have_devices):
|
||||
print('')
|
||||
if len(discovered_devices) == 0:
|
||||
if have_devices:
|
||||
print('Connect your ODrive to {} and power it up.'.format(args.path))
|
||||
print('After that, the following message should appear:')
|
||||
print(' "Connected to ODrive [serial number] as odrv0"')
|
||||
@@ -29,41 +30,41 @@ def print_help(args):
|
||||
print('')
|
||||
|
||||
|
||||
interactive_variables = {}
|
||||
|
||||
discovered_devices = []
|
||||
|
||||
def did_discover_device(odrive, logger, app_shutdown_token):
|
||||
"""
|
||||
Handles the discovery of new devices by displaying a
|
||||
message and making the device available to the interactive
|
||||
console
|
||||
"""
|
||||
serial_number = odrive.serial_number if hasattr(odrive, 'serial_number') else "[unknown serial number]"
|
||||
if serial_number in discovered_devices:
|
||||
verb = "Reconnected"
|
||||
index = discovered_devices.index(serial_number)
|
||||
else:
|
||||
verb = "Connected"
|
||||
discovered_devices.append(serial_number)
|
||||
index = len(discovered_devices) - 1
|
||||
interactive_name = "odrv" + str(index)
|
||||
|
||||
# Publish new ODrive to interactive console
|
||||
interactive_variables[interactive_name] = odrive
|
||||
globals()[interactive_name] = odrive # Add to globals so tab complete works
|
||||
logger.info("{} to ODrive {:012X} as {}".format(verb, serial_number, interactive_name))
|
||||
|
||||
# Subscribe to disappearance of the device
|
||||
odrive.__channel__._channel_broken.subscribe(lambda: did_lose_device(interactive_name, logger, app_shutdown_token))
|
||||
|
||||
def did_lose_device(interactive_name, logger, app_shutdown_token):
|
||||
"""
|
||||
Handles the disappearance of a device by displaying
|
||||
a message.
|
||||
"""
|
||||
if not app_shutdown_token.is_set():
|
||||
logger.warn("Oh no {} disappeared".format(interactive_name))
|
||||
#interactive_variables = {}
|
||||
#
|
||||
#discovered_devices = []
|
||||
#
|
||||
#def did_discover_device(odrive, logger, app_shutdown_token):
|
||||
# """
|
||||
# Handles the discovery of new devices by displaying a
|
||||
# message and making the device available to the interactive
|
||||
# console
|
||||
# """
|
||||
# serial_number = odrive.serial_number if hasattr(odrive, 'serial_number') else "[unknown serial number]"
|
||||
# if serial_number in discovered_devices:
|
||||
# verb = "Reconnected"
|
||||
# index = discovered_devices.index(serial_number)
|
||||
# else:
|
||||
# verb = "Connected"
|
||||
# discovered_devices.append(serial_number)
|
||||
# index = len(discovered_devices) - 1
|
||||
# interactive_name = "odrv" + str(index)
|
||||
#
|
||||
# # Publish new ODrive to interactive console
|
||||
# interactive_variables[interactive_name] = odrive
|
||||
# globals()[interactive_name] = odrive # Add to globals so tab complete works
|
||||
# logger.info("{} to ODrive {:012X} as {}".format(verb, serial_number, interactive_name))
|
||||
#
|
||||
# # Subscribe to disappearance of the device
|
||||
# odrive.__channel__._channel_broken.subscribe(lambda: did_lose_device(interactive_name, logger, app_shutdown_token))
|
||||
#
|
||||
#def did_lose_device(interactive_name, logger, app_shutdown_token):
|
||||
# """
|
||||
# Handles the disappearance of a device by displaying
|
||||
# a message.
|
||||
# """
|
||||
# if not app_shutdown_token.is_set():
|
||||
# logger.warn("Oh no {} disappeared".format(interactive_name))
|
||||
|
||||
def launch_shell(args, logger, printer, app_shutdown_token):
|
||||
"""
|
||||
@@ -73,61 +74,7 @@ def launch_shell(args, logger, printer, app_shutdown_token):
|
||||
"odrv0", "odrv1", ...
|
||||
"""
|
||||
|
||||
# Connect to device
|
||||
logger.debug("Waiting for device...")
|
||||
odrive.discovery.find_all(args.path, args.serial_number,
|
||||
lambda dev: did_discover_device(dev, logger, app_shutdown_token),
|
||||
app_shutdown_token,
|
||||
app_shutdown_token,
|
||||
printer=printer)
|
||||
|
||||
# Check if IPython is installed
|
||||
if args.no_ipython:
|
||||
use_ipython = False
|
||||
else:
|
||||
try:
|
||||
import IPython
|
||||
use_ipython = True
|
||||
except:
|
||||
print("Warning: you don't have IPython installed.")
|
||||
print("If you want to have an improved interactive console with pretty colors,")
|
||||
print("you should install IPython\n")
|
||||
use_ipython = False
|
||||
|
||||
interactive_variables["help"] = lambda: print_help(args)
|
||||
|
||||
# If IPython is installed, embed IPython shell, otherwise embed regular shell
|
||||
if use_ipython:
|
||||
help = lambda: print_help(args) # Override help function # pylint: disable=W0612
|
||||
console = IPython.terminal.embed.InteractiveShellEmbed(banner1='')
|
||||
console.runcode = console.run_code # hack to make IPython look like the regular console
|
||||
interact = console
|
||||
else:
|
||||
# Enable tab complete if possible
|
||||
try:
|
||||
import readline # Works only on Unix
|
||||
readline.parse_and_bind("tab: complete")
|
||||
except:
|
||||
sudo_prefix = "" if platform.system() == "Windows" else "sudo "
|
||||
print("Warning: could not enable tab-complete. User experience will suffer.\n"
|
||||
"Run `{}pip install readline` and then restart this script to fix this."
|
||||
.format(sudo_prefix))
|
||||
|
||||
import code
|
||||
console = code.InteractiveConsole(locals=interactive_variables)
|
||||
interact = lambda: console.interact(banner='')
|
||||
|
||||
# install hook to hide ChannelBrokenException
|
||||
console.runcode('import sys')
|
||||
console.runcode('superexcepthook = sys.excepthook')
|
||||
console.runcode('def newexcepthook(ex_class,ex,trace):\n'
|
||||
' if ex_class.__module__ + "." + ex_class.__name__ != "odrive.protocol.ChannelBrokenException":\n'
|
||||
' superexcepthook(ex_class,ex,trace)')
|
||||
console.runcode('sys.excepthook=newexcepthook')
|
||||
|
||||
|
||||
# Launch shell
|
||||
print_banner()
|
||||
logger._skip_bottom_line = True
|
||||
interact()
|
||||
app_shutdown_token.set()
|
||||
fibre.launch_shell(args,
|
||||
print_banner, print_help,
|
||||
logger, app_shutdown_token,
|
||||
branding_short="odrv", branding_long="ODrive")
|
||||
|
||||
@@ -5,7 +5,8 @@ import math
|
||||
import time
|
||||
import sys
|
||||
import threading
|
||||
import odrive.discovery
|
||||
import fibre
|
||||
import odrive
|
||||
from odrive.enums import *
|
||||
import odrive.utils
|
||||
import numpy as np
|
||||
@@ -37,7 +38,7 @@ class ODriveTestContext():
|
||||
"""
|
||||
Reconnects to the ODrive
|
||||
"""
|
||||
self.handle = odrive.discovery.find_any(
|
||||
self.handle = odrive.find_any(
|
||||
path="usb", serial_number=self.yaml['serial-number'], timeout=15)#, printer=print)
|
||||
for axis_idx, axis_ctx in enumerate(self.axes):
|
||||
axis_ctx.handle = self.handle.__dict__['axis{}'.format(axis_idx)]
|
||||
@@ -250,7 +251,7 @@ class TestFlashAndErase(ODriveTest):
|
||||
# this is a firmware issue since it persists when unplugging/replugging
|
||||
# but goes away when power cycling the device
|
||||
odrv_ctx.handle.reboot()
|
||||
except odrive.protocol.ChannelBrokenException:
|
||||
except fibre.ChannelBrokenException:
|
||||
pass # this is expected
|
||||
time.sleep(0.5)
|
||||
|
||||
@@ -393,7 +394,7 @@ class TestStoreAndReboot(ODriveTest):
|
||||
odrv_ctx.handle.save_configuration()
|
||||
try:
|
||||
odrv_ctx.handle.reboot()
|
||||
except odrive.protocol.ChannelBrokenException:
|
||||
except fibre.ChannelBrokenException:
|
||||
pass # this is expected
|
||||
time.sleep(2)
|
||||
|
||||
|
||||
@@ -1,195 +0,0 @@
|
||||
# requires pyusb
|
||||
# pip install --pre pyusb
|
||||
|
||||
import sys
|
||||
import time
|
||||
import usb.core
|
||||
import usb.util
|
||||
import odrive.protocol
|
||||
import traceback
|
||||
import platform
|
||||
|
||||
ODRIVE_VID_PID_PAIRS = [
|
||||
(0x1209, 0x0D31),
|
||||
(0x1209, 0x0D32), # <== TODO: this is the only official ODrive PID, remove the other ones
|
||||
(0x1209, 0x0D33)
|
||||
]
|
||||
|
||||
class USBBulkTransport(odrive.protocol.PacketSource, odrive.protocol.PacketSink):
|
||||
def __init__(self, dev, printer):
|
||||
self._printer = printer
|
||||
self.dev = dev
|
||||
self.intf = None
|
||||
self._name = "USB device {}:{}".format(dev.idVendor, dev.idProduct)
|
||||
self._was_damaged = False
|
||||
|
||||
##
|
||||
# information about the connected device
|
||||
##
|
||||
def info(self):
|
||||
# loop through configurations
|
||||
string = ""
|
||||
for cfg in self.dev:
|
||||
string += "ConfigurationValue {0}\n".format(cfg.bConfigurationValue)
|
||||
for intf in cfg:
|
||||
string += "\tInterfaceNumber {0},{1}\n".format(intf.bInterfaceNumber, intf.bAlternateSetting)
|
||||
for ep in intf:
|
||||
string += "\t\tEndpointAddress {0}\n".format(ep.bEndpointAddress)
|
||||
return string
|
||||
|
||||
def init(self):
|
||||
# Under some conditions, the Linux USB/libusb stack ends up in a corrupt
|
||||
# state where there are a few packets in a receive queue but a call
|
||||
# to epr.read() does not return these packet until a new packet arrives.
|
||||
# This undesirable queue can be cleared by resetting the device.
|
||||
# On windows this would cause file-not-found errors in subsequent dev calls
|
||||
if platform.system() != 'Windows':
|
||||
self.dev.reset()
|
||||
|
||||
interface_number = 1
|
||||
try:
|
||||
if self.dev.is_kernel_driver_active(interface_number):
|
||||
self.dev.detach_kernel_driver(interface_number)
|
||||
self._printer("Detached Kernel Driver")
|
||||
except NotImplementedError:
|
||||
pass #is_kernel_driver_active not implemented on Windows
|
||||
|
||||
self.dev.set_configuration() # no args: set first configuration
|
||||
self.cfg = self.dev.get_active_configuration()
|
||||
self.intf = self.cfg[(1,0)] # this implicitly claims the interface
|
||||
# write endpoint
|
||||
self.epw = usb.util.find_descriptor(self.intf,
|
||||
# match the first OUT endpoint
|
||||
custom_match = \
|
||||
lambda e: \
|
||||
usb.util.endpoint_direction(e.bEndpointAddress) == \
|
||||
usb.util.ENDPOINT_OUT
|
||||
)
|
||||
assert self.epw is not None
|
||||
self._printer("EndpointAddress for writing {}".format(self.epw.bEndpointAddress))
|
||||
# read endpoint
|
||||
self.epr = usb.util.find_descriptor(self.intf,
|
||||
# match the first IN endpoint
|
||||
custom_match = \
|
||||
lambda e: \
|
||||
usb.util.endpoint_direction(e.bEndpointAddress) == \
|
||||
usb.util.ENDPOINT_IN
|
||||
)
|
||||
assert self.epr is not None
|
||||
self._printer("EndpointAddress for reading {}".format(self.epr.bEndpointAddress))
|
||||
|
||||
def deinit(self):
|
||||
if not self.intf is None:
|
||||
usb.util.release_interface(self.dev, self.intf)
|
||||
|
||||
def process_packet(self, usbBuffer):
|
||||
try:
|
||||
ret = self.epw.write(usbBuffer, 0)
|
||||
if self._was_damaged:
|
||||
self._printer("Recovered from USB halt/stall condition")
|
||||
self._was_damaged = False
|
||||
return ret
|
||||
except usb.core.USBError as ex:
|
||||
if ex.errno == 19: # "no such device"
|
||||
raise odrive.protocol.ChannelBrokenException()
|
||||
elif ex.errno == 110: # timeout
|
||||
raise odrive.utils.TimeoutException()
|
||||
else:
|
||||
self._printer("halt condition: {}".format(ex.errno))
|
||||
# Try resetting halt/stall condition
|
||||
try:
|
||||
self.deinit()
|
||||
self.init()
|
||||
except usb.core.USBError:
|
||||
raise odrive.protocol.ChannelBrokenException()
|
||||
# Retry transfer
|
||||
self._was_damaged = True
|
||||
raise odrive.protocol.ChannelDamagedException()
|
||||
|
||||
def get_packet(self, deadline):
|
||||
try:
|
||||
bufferLen = self.epr.wMaxPacketSize
|
||||
timeout = max(int((deadline - time.monotonic()) * 1000), 0)
|
||||
ret = self.epr.read(bufferLen, timeout)
|
||||
if self._was_damaged:
|
||||
self._printer("Recovered from USB halt/stall condition")
|
||||
self._was_damaged = False
|
||||
return bytearray(ret)
|
||||
except usb.core.USBError as ex:
|
||||
if ex.errno == 19: # "no such device"
|
||||
raise odrive.protocol.ChannelBrokenException()
|
||||
elif ex.errno is None or ex.errno == 110: # timeout
|
||||
raise odrive.utils.TimeoutException()
|
||||
else:
|
||||
self._printer("halt condition: {}".format(ex.errno))
|
||||
# Try resetting halt/stall condition
|
||||
try:
|
||||
self.deinit()
|
||||
self.init()
|
||||
except usb.core.USBError:
|
||||
raise odrive.protocol.ChannelBrokenException()
|
||||
# Retry transfer
|
||||
self._was_damaged = True
|
||||
raise odrive.protocol.ChannelDamagedException()
|
||||
|
||||
|
||||
def discover_channels(path, serial_number, callback, cancellation_token, channel_termination_token, printer):
|
||||
"""
|
||||
Scans for USB devices that match the path spec.
|
||||
This function blocks until cancellation_token is set.
|
||||
Channels spawned by this function run until channel_termination_token is set.
|
||||
"""
|
||||
if path == None or path == "":
|
||||
bus = None
|
||||
address = None
|
||||
else:
|
||||
try:
|
||||
bus = int(path.split(":")[0])
|
||||
address = int(path.split(":")[1])
|
||||
except (ValueError, IndexError):
|
||||
raise Exception("{} is not a valid USB path specification. "
|
||||
"Expected a string of the format BUS:DEVICE where BUS "
|
||||
"and DEVICE are integers.".format(path))
|
||||
|
||||
known_devices = []
|
||||
def device_matcher(device):
|
||||
#print(" test {:04X}:{:04X}".format(device.idVendor, device.idProduct))
|
||||
if (device.bus, device.address) in known_devices:
|
||||
return False
|
||||
if bus != None and device.bus != bus:
|
||||
return False
|
||||
if address != None and device.address != address:
|
||||
return False
|
||||
if serial_number != None and device.serial_number != serial_number:
|
||||
return False
|
||||
if (device.idVendor, device.idProduct) not in ODRIVE_VID_PID_PAIRS:
|
||||
return False
|
||||
return True
|
||||
|
||||
while not cancellation_token.is_set():
|
||||
# printer("USB discover loop")
|
||||
devices = usb.core.find(find_all=True, custom_match=device_matcher)
|
||||
for usb_device in devices:
|
||||
try:
|
||||
bulk_device = USBBulkTransport(usb_device, printer)
|
||||
printer(bulk_device.info())
|
||||
bulk_device.init()
|
||||
channel = odrive.protocol.Channel(
|
||||
"USB device bus {} device {}".format(usb_device.bus, usb_device.address),
|
||||
bulk_device, bulk_device, channel_termination_token, printer)
|
||||
channel.usb_device = usb_device # for debugging only
|
||||
except usb.core.USBError as ex:
|
||||
if ex.errno == 13:
|
||||
printer("USB device access denied. Did you set up your udev rules correctly?")
|
||||
continue
|
||||
elif ex.errno == 16:
|
||||
printer("USB device busy. I'll reset it and try again.")
|
||||
usb_device.reset()
|
||||
continue
|
||||
else:
|
||||
printer("USB device init failed. Ignoring this device. More info: " + traceback.format_exc())
|
||||
known_devices.append((usb_device.bus, usb_device.address))
|
||||
else:
|
||||
known_devices.append((usb_device.bus, usb_device.address))
|
||||
callback(channel)
|
||||
time.sleep(1)
|
||||
+1
-211
@@ -1,7 +1,3 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
Liveplotter
|
||||
"""
|
||||
|
||||
import sys
|
||||
import time
|
||||
@@ -9,6 +5,7 @@ import threading
|
||||
import platform
|
||||
import subprocess
|
||||
import os
|
||||
from fibre.utils import Event
|
||||
|
||||
try:
|
||||
if platform.system() == 'Windows':
|
||||
@@ -156,210 +153,3 @@ def setup_udev_rules(logger):
|
||||
subprocess.run(["udevadm", "control", "--reload-rules"], check=True)
|
||||
subprocess.run(["udevadm", "trigger"], check=True)
|
||||
logger.info('udev rules configured successfully')
|
||||
|
||||
|
||||
## Exceptions ##
|
||||
|
||||
class TimeoutException(Exception):
|
||||
pass
|
||||
|
||||
## Threading utils ##
|
||||
|
||||
class Event():
|
||||
"""
|
||||
Alternative to threading.Event(), enhanced by the subscribe() function
|
||||
that the original fails to provide.
|
||||
@param Trigger: if supplied, the newly created event will be triggered
|
||||
as soon as the trigger event becomes set
|
||||
"""
|
||||
def __init__(self, trigger=None):
|
||||
self._evt = threading.Event()
|
||||
self._subscribers = []
|
||||
self._mutex = threading.Lock()
|
||||
if not trigger is None:
|
||||
trigger.subscribe(lambda: self.set())
|
||||
|
||||
def is_set(self):
|
||||
return self._evt.is_set()
|
||||
|
||||
def set(self):
|
||||
"""
|
||||
Sets the event and invokes all subscribers if the event was
|
||||
not already set
|
||||
"""
|
||||
self._mutex.acquire()
|
||||
try:
|
||||
if not self._evt.is_set():
|
||||
self._evt.set()
|
||||
for s in self._subscribers:
|
||||
s()
|
||||
finally:
|
||||
self._mutex.release()
|
||||
|
||||
def subscribe(self, handler):
|
||||
"""
|
||||
Invokes the specified handler exactly once as soon as the
|
||||
specified event is set. If the event is already set, the
|
||||
handler is invoked immediately.
|
||||
Returns a function that can be invoked to unsubscribe.
|
||||
"""
|
||||
if handler is None:
|
||||
raise TypeError
|
||||
self._mutex.acquire()
|
||||
try:
|
||||
self._subscribers.append(handler)
|
||||
if self._evt.is_set():
|
||||
handler()
|
||||
finally:
|
||||
self._mutex.release()
|
||||
return handler
|
||||
|
||||
def unsubscribe(self, handler):
|
||||
self._mutex.acquire()
|
||||
try:
|
||||
self._subscribers.pop(self._subscribers.index(handler))
|
||||
finally:
|
||||
self._mutex.release()
|
||||
|
||||
def wait(self, timeout=None):
|
||||
if not self._evt.wait(timeout=timeout):
|
||||
raise TimeoutError()
|
||||
|
||||
def trigger_after(self, timeout):
|
||||
"""
|
||||
Triggers the event after the specified timeout.
|
||||
This function returns immediately.
|
||||
"""
|
||||
def delayed_trigger():
|
||||
if not self.wait(timeout=timeout):
|
||||
self.set()
|
||||
threading.Thread(target=delayed_trigger, daemon=True).start()
|
||||
|
||||
def wait_any(timeout=None, *events):
|
||||
"""
|
||||
Blocks until any of the specified events are triggered.
|
||||
Returns the index of the event that was triggerd or raises
|
||||
a TimeoutException
|
||||
Param timeout: A timeout in seconds
|
||||
"""
|
||||
or_event = threading.Event()
|
||||
subscriptions = []
|
||||
for event in events:
|
||||
subscriptions.append((event, event.subscribe(lambda: or_event.set())))
|
||||
or_event.wait(timeout=timeout)
|
||||
for event, sub in subscriptions:
|
||||
event.unsubscribe(sub)
|
||||
for i in range(len(events)):
|
||||
if events[i].is_set():
|
||||
return i
|
||||
raise TimeoutException()
|
||||
|
||||
|
||||
class Logger():
|
||||
"""
|
||||
Logs messages to stdout
|
||||
"""
|
||||
|
||||
COLOR_DEFAULT = 0
|
||||
COLOR_GREEN = 1
|
||||
COLOR_CYAN = 2
|
||||
COLOR_YELLOW = 3
|
||||
COLOR_RED = 4
|
||||
|
||||
_VT100Colors = {
|
||||
COLOR_GREEN: '\x1b[92;1m',
|
||||
COLOR_CYAN: '\x1b[96;1m',
|
||||
COLOR_YELLOW: '\x1b[93;1m',
|
||||
COLOR_RED: '\x1b[91;1m',
|
||||
COLOR_DEFAULT: '\x1b[0m'
|
||||
}
|
||||
|
||||
_Win32Colors = {
|
||||
COLOR_GREEN: 0x0A,
|
||||
COLOR_CYAN: 0x0B,
|
||||
COLOR_YELLOW: 0x0E,
|
||||
COLOR_RED: 0x0C,
|
||||
COLOR_DEFAULT: 0x07
|
||||
}
|
||||
|
||||
def __init__(self, verbose=True):
|
||||
self._prefix = ''
|
||||
self._skip_bottom_line = False # If true, messages are printed one line above the cursor
|
||||
self._verbose = verbose
|
||||
self._print_lock = threading.Lock()
|
||||
if platform.system() == 'Windows':
|
||||
self._stdout_buf = win32console.GetStdHandle(win32console.STD_OUTPUT_HANDLE)
|
||||
|
||||
def indent(self, prefix=' '):
|
||||
indented_logger = Logger()
|
||||
indented_logger._prefix = self._prefix + prefix
|
||||
return indented_logger
|
||||
|
||||
def print_on_second_last_line(self, text, color):
|
||||
"""
|
||||
Prints a text on the second last line.
|
||||
This can be used to print a message above the command
|
||||
prompt. If the command prompt spans multiple lines
|
||||
there will be glitches.
|
||||
If the printed text spans multiple lines there will also
|
||||
be glitches (though this could be fixed).
|
||||
"""
|
||||
|
||||
if platform.system() == 'Windows':
|
||||
# Windows <10 doesn't understand VT100 escape codes and the colorama
|
||||
# also doesn't support the specific escape codes we need so we use the
|
||||
# native Win32 API.
|
||||
info = self._stdout_buf.GetConsoleScreenBufferInfo()
|
||||
cursor_pos = info['CursorPosition']
|
||||
scroll_rect=win32console.PySMALL_RECTType(
|
||||
Left=0, Top=1,
|
||||
Right=info['Window'].Right,
|
||||
Bottom=cursor_pos.Y-1)
|
||||
scroll_dest = win32console.PyCOORDType(scroll_rect.Left, scroll_rect.Top-1)
|
||||
self._stdout_buf.ScrollConsoleScreenBuffer(
|
||||
scroll_rect, scroll_rect, scroll_dest, # clipping rect is same as scroll rect
|
||||
u' ', Logger._Win32Colors[color]) # fill with empty cells with the desired color attributes
|
||||
line_start = win32console.PyCOORDType(0, cursor_pos.Y-1)
|
||||
self._stdout_buf.WriteConsoleOutputCharacter(text, line_start)
|
||||
|
||||
else:
|
||||
# Assume we're in a terminal that interprets VT100 escape codes.
|
||||
# TODO: test on macOS
|
||||
|
||||
# Escape character sequence:
|
||||
# ESC 7: store cursor position
|
||||
# ESC 1A: move cursor up by one
|
||||
# ESC 1S: scroll entire viewport by one
|
||||
# ESC 1L: insert 1 line at cursor position
|
||||
# (print text)
|
||||
# ESC 8: restore old cursor position
|
||||
|
||||
self._print_lock.acquire()
|
||||
sys.stdout.write('\x1b7\x1b[1A\x1b[1S\x1b[1L')
|
||||
sys.stdout.write(Logger._VT100Colors[color] + text + Logger._VT100Colors[Logger.COLOR_DEFAULT])
|
||||
sys.stdout.write('\x1b8')
|
||||
sys.stdout.flush()
|
||||
self._print_lock.release()
|
||||
|
||||
def print_colored(self, text, color):
|
||||
if self._skip_bottom_line:
|
||||
self.print_on_second_last_line(text, color)
|
||||
else:
|
||||
# On Windows, colorama does the job of interpreting the VT100 escape sequences
|
||||
self._print_lock.acquire()
|
||||
sys.stdout.write(Logger._VT100Colors[color] + text + Logger._VT100Colors[Logger.COLOR_DEFAULT] + '\n')
|
||||
sys.stdout.flush()
|
||||
self._print_lock.release()
|
||||
|
||||
def debug(self, text):
|
||||
if self._verbose:
|
||||
self.print_colored(self._prefix + text, Logger.COLOR_DEFAULT)
|
||||
def success(self, text):
|
||||
self.print_colored(self._prefix + text, Logger.COLOR_GREEN)
|
||||
def info(self, text):
|
||||
self.print_colored(self._prefix + text, Logger.COLOR_CYAN)
|
||||
def warn(self, text):
|
||||
self.print_colored(self._prefix + text, Logger.COLOR_YELLOW)
|
||||
def error(self, text):
|
||||
# TODO: write to stderr
|
||||
self.print_colored(self._prefix + text, Logger.COLOR_RED)
|
||||
|
||||
@@ -5,15 +5,15 @@ Example usage of the ODrive python library to monitor and control ODrive devices
|
||||
|
||||
from __future__ import print_function
|
||||
|
||||
import odrive.discovery
|
||||
import odrive
|
||||
import time
|
||||
import math
|
||||
|
||||
# Find a connected ODrive (this will block until you connect one)
|
||||
my_drive = odrive.discovery.find_any()
|
||||
my_drive = odrive.find_any()
|
||||
|
||||
# Find an ODrive that is connected on the serial port /dev/ttyUSB0
|
||||
#my_drive = odrive.discovery.find_any("serial:/dev/ttyUSB0")
|
||||
#my_drive = odrive.find_any("serial:/dev/ttyUSB0")
|
||||
|
||||
# The above call returns a python object with a dynamically generated type. The
|
||||
# type hierarchy will correspond to the endpoint list in `MotorControl/protocol.cpp`.
|
||||
|
||||
+8
-5
@@ -6,8 +6,11 @@ ODrive command line utility
|
||||
from __future__ import print_function
|
||||
import sys
|
||||
import argparse
|
||||
import odrive.discovery
|
||||
from odrive.utils import Logger, Event
|
||||
import odrive
|
||||
import fibre.discovery
|
||||
from fibre.utils import Logger, Event
|
||||
|
||||
#print("Refer to install instructions at http://docs.odriverobotics.com/#downloading-and-installing-tools")
|
||||
|
||||
# Flush stdout by default
|
||||
# Source:
|
||||
@@ -115,7 +118,7 @@ try:
|
||||
elif args.command == 'liveplotter':
|
||||
from odrive.utils import start_liveplotter
|
||||
print("Waiting for ODrive...")
|
||||
my_odrive = odrive.discovery.find_any(path=args.path, serial_number=args.serial_number)
|
||||
my_odrive = odrive.find_any(path=args.path, serial_number=args.serial_number)
|
||||
|
||||
# If you want to plot different values, change them here.
|
||||
# You can plot any number of values concurrently.
|
||||
@@ -125,14 +128,14 @@ try:
|
||||
elif args.command == 'drv-status':
|
||||
from odrive.utils import print_drv_regs
|
||||
print("Waiting for ODrive...")
|
||||
my_odrive = odrive.discovery.find_any(path=args.path, serial_number=args.serial_number)
|
||||
my_odrive = odrive.find_any(path=args.path, serial_number=args.serial_number)
|
||||
print_drv_regs("Motor 0", my_odrive.axis0.motor)
|
||||
print_drv_regs("Motor 1", my_odrive.axis1.motor)
|
||||
|
||||
elif args.command == 'rate-test':
|
||||
from odrive.utils import rate_test
|
||||
print("Waiting for ODrive...")
|
||||
my_odrive = odrive.discovery.find_any(path=args.path, serial_number=args.serial_number)
|
||||
my_odrive = odrive.find_any(path=args.path, serial_number=args.serial_number)
|
||||
rate_test(my_odrive)
|
||||
|
||||
elif args.command == 'udev-setup':
|
||||
|
||||
Reference in New Issue
Block a user