mirror of
https://github.com/undera/pylgbst.git
synced 2020-11-18 19:37:26 -08:00
Merge branch 'master' of github.com:undera/pylgbst
This commit is contained in:
Andrey Pohilko
commit
4e8708453a
@ -108,6 +108,7 @@ class MoveHub(object):
|
||||
self.devices[port].finished()
|
||||
elif status == STATUS_CONFLICT:
|
||||
log.warning("Command conflict on port %s", PORTS[port])
|
||||
self.devices[port].finished()
|
||||
else:
|
||||
log.warning("Unhandled status value: 0x%x", status)
|
||||
|
||||
|
||||
@ -1,13 +1,16 @@
|
||||
"""
|
||||
This package holds communication aspects
|
||||
"""
|
||||
import binascii
|
||||
import json
|
||||
import logging
|
||||
import socket
|
||||
import sys
|
||||
import time
|
||||
import traceback
|
||||
from Queue import Queue
|
||||
from abc import abstractmethod
|
||||
from binascii import unhexlify
|
||||
from gattlib import DiscoveryService, GATTRequester
|
||||
from threading import Thread
|
||||
|
||||
@ -15,29 +18,15 @@ from pylgbst.constants import LEGO_MOVE_HUB, MSG_DEVICE_SHUTDOWN
|
||||
|
||||
log = logging.getLogger('transport')
|
||||
|
||||
|
||||
def str2hex(data): # TODO: eliminate it
|
||||
return binascii.hexlify(data).decode("utf8")
|
||||
|
||||
|
||||
if sys.version_info[0] == 2:
|
||||
def str2hex(data):
|
||||
return data.encode("hex")
|
||||
|
||||
|
||||
def hex2str(data):
|
||||
return data.decode("hex")
|
||||
|
||||
|
||||
def get_byte(seq, index):
|
||||
return ord(seq[index])
|
||||
else:
|
||||
import binascii
|
||||
|
||||
|
||||
def str2hex(data):
|
||||
return binascii.hexlify(data).decode("utf8")
|
||||
|
||||
|
||||
def hex2str(data):
|
||||
return binascii.unhexlify(data)
|
||||
|
||||
|
||||
def get_byte(seq, index):
|
||||
return seq[index]
|
||||
|
||||
@ -53,14 +42,31 @@ class Requester(GATTRequester):
|
||||
super(Requester, self).__init__(p_object, *args, **kwargs)
|
||||
self.notification_sink = None
|
||||
|
||||
self._notify_queue = Queue()
|
||||
self._notifier_thread = Thread(target=self._dispatch_notifications)
|
||||
self._notifier_thread.setDaemon(True)
|
||||
self._notifier_thread.setName("Notify queue dispatcher")
|
||||
self._notifier_thread.start()
|
||||
|
||||
def on_notification(self, handle, data):
|
||||
# log.debug("requester notified, sink: %s", self.notification_sink)
|
||||
if self.notification_sink:
|
||||
self.notification_sink(handle, data)
|
||||
self._notify_queue.put((handle, data))
|
||||
|
||||
def on_indication(self, handle, data):
|
||||
log.debug("Indication on handle %s: %s", handle, str2hex(data))
|
||||
|
||||
def _dispatch_notifications(self):
|
||||
while True:
|
||||
handle, data = self._notify_queue.get()
|
||||
if self.notification_sink:
|
||||
try:
|
||||
self.notification_sink(handle, data)
|
||||
except BaseException:
|
||||
log.warning("Failed to dispatch notification: %s", str2hex(data))
|
||||
log.warning("Failed to dispatch notification: %s", traceback.format_exc())
|
||||
else:
|
||||
log.warning("Dropped notification %s: %s", handle, str2hex(data))
|
||||
|
||||
|
||||
class Connection(object):
|
||||
@abstractmethod
|
||||
@ -81,17 +87,17 @@ class BLEConnection(Connection):
|
||||
Main transport class, uses real Bluetooth LE connection.
|
||||
Loops with timeout of 1 seconds to find device named "Lego MOVE Hub"
|
||||
|
||||
:type requester: Requester
|
||||
:type _requester: Requester
|
||||
"""
|
||||
|
||||
def __init__(self):
|
||||
super(BLEConnection, self).__init__()
|
||||
self.requester = None
|
||||
self._requester = None
|
||||
|
||||
def connect(self, bt_iface_name='hci0'):
|
||||
service = DiscoveryService(bt_iface_name)
|
||||
|
||||
while not self.requester:
|
||||
while not self._requester:
|
||||
log.info("Discovering devices using %s...", bt_iface_name)
|
||||
devices = service.discover(1)
|
||||
log.debug("Devices: %s", devices)
|
||||
@ -99,25 +105,22 @@ class BLEConnection(Connection):
|
||||
for address, name in devices.items():
|
||||
if name == LEGO_MOVE_HUB:
|
||||
logging.info("Found %s at %s", name, address)
|
||||
self._get_requester(address, bt_iface_name)
|
||||
self._requester = Requester(address, True, bt_iface_name)
|
||||
break
|
||||
|
||||
return self
|
||||
|
||||
def _get_requester(self, address, bt_iface_name):
|
||||
self.requester = Requester(address, True, bt_iface_name)
|
||||
|
||||
def set_notify_handler(self, handler):
|
||||
if self.requester:
|
||||
if self._requester:
|
||||
log.debug("Setting notification handler: %s", handler)
|
||||
self.requester.notification_sink = handler
|
||||
self._requester.notification_sink = handler
|
||||
else:
|
||||
raise RuntimeError("No requester available")
|
||||
|
||||
def read(self, handle):
|
||||
# FIXME: repeating reads hangs it...
|
||||
log.debug("Reading from: %s", handle)
|
||||
data = self.requester.read_by_handle(handle)
|
||||
data = self._requester.read_by_handle(handle)
|
||||
log.debug("Result: %s", data)
|
||||
if isinstance(data, list):
|
||||
data = data[0]
|
||||
@ -125,7 +128,7 @@ class BLEConnection(Connection):
|
||||
|
||||
def write(self, handle, data):
|
||||
log.debug("Writing to %s: %s", handle, str2hex(data))
|
||||
return self.requester.write_by_handle(handle, data)
|
||||
return self._requester.write_by_handle(handle, data)
|
||||
|
||||
|
||||
class DebugServer(object):
|
||||
@ -152,7 +155,7 @@ class DebugServer(object):
|
||||
conn, addr = self.sock.accept()
|
||||
if not self._running:
|
||||
raise KeyboardInterrupt("Shutdown")
|
||||
self.ble.requester.notification_sink = lambda x, y: self._notify(conn, x, y)
|
||||
self.ble._requester.notification_sink = lambda x, y: self._notify(conn, x, y)
|
||||
try:
|
||||
self._handle_conn(conn)
|
||||
except KeyboardInterrupt:
|
||||
@ -160,14 +163,14 @@ class DebugServer(object):
|
||||
except BaseException:
|
||||
log.error("Problem handling incoming connection: %s", traceback.format_exc())
|
||||
finally:
|
||||
self.ble.requester.notification_sink = self._notify_dummy
|
||||
self.ble._requester.notification_sink = self._notify_dummy
|
||||
conn.close()
|
||||
|
||||
def __del__(self):
|
||||
self.sock.close()
|
||||
|
||||
def _notify_dummy(self, handle, data):
|
||||
log.debug("Notification from handle %s: %s", handle, hex2str(data))
|
||||
log.debug("Notification from handle %s: %s", handle, hexlify(data.strip()))
|
||||
self._check_shutdown(data)
|
||||
|
||||
def _notify(self, conn, handle, data):
|
||||
@ -215,7 +218,7 @@ class DebugServer(object):
|
||||
|
||||
def _handle_cmd(self, cmd):
|
||||
if cmd['type'] == 'write':
|
||||
self.ble.write(cmd['handle'], hex2str(cmd['data']))
|
||||
self.ble.write(cmd['handle'], unhexlify(cmd['data']))
|
||||
elif cmd['type'] == 'read':
|
||||
data = self.ble.read(cmd['handle'])
|
||||
payload = {"type": "response", "data": str2hex(data)}
|
||||
@ -239,6 +242,7 @@ class DebugServerConnection(Connection):
|
||||
self.incoming = []
|
||||
|
||||
self.reader = Thread(target=self._recv)
|
||||
self.reader.setName("Debug connection reader")
|
||||
self.reader.setDaemon(True)
|
||||
self.reader.start()
|
||||
|
||||
@ -264,7 +268,7 @@ class DebugServerConnection(Connection):
|
||||
for item in self.incoming:
|
||||
if item['type'] == 'response':
|
||||
self.incoming.remove(item)
|
||||
return hex2str(item['data'])
|
||||
return unhexlify(item['data'])
|
||||
time.sleep(0.1)
|
||||
|
||||
def _send(self, payload):
|
||||
@ -276,7 +280,7 @@ class DebugServerConnection(Connection):
|
||||
data = self.sock.recv(1024)
|
||||
log.debug("Recv from debug server: %s", data.strip())
|
||||
if not data:
|
||||
break
|
||||
raise KeyboardInterrupt("Server has closed connection")
|
||||
|
||||
self.buf += data
|
||||
|
||||
@ -286,7 +290,7 @@ class DebugServerConnection(Connection):
|
||||
if line:
|
||||
item = json.loads(line)
|
||||
if item['type'] == 'notification' and self.notify_handler:
|
||||
self.notify_handler(item['handle'], hex2str(item['data']))
|
||||
self.notify_handler(item['handle'], unhexlify(item['data']))
|
||||
elif item['type'] == 'response':
|
||||
self.incoming.append(item)
|
||||
else:
|
||||
@ -294,3 +298,10 @@ class DebugServerConnection(Connection):
|
||||
|
||||
def set_notify_handler(self, handler):
|
||||
self.notify_handler = handler
|
||||
|
||||
|
||||
def start_debug_server(iface="hci0", port=9090):
|
||||
ble = BLEConnection()
|
||||
ble.connect(iface)
|
||||
server = DebugServer(ble)
|
||||
server.start(port)
|
||||
|
||||
@ -126,7 +126,6 @@ class EncodedMotor(Peripheral):
|
||||
|
||||
command += self.TRAILER
|
||||
|
||||
self._working = -1
|
||||
self._write_to_hub(MSG_SET_PORT_VAL, command)
|
||||
|
||||
def timed(self, seconds, speed_primary=1, speed_secondary=None, async=False):
|
||||
@ -141,6 +140,7 @@ class EncodedMotor(Peripheral):
|
||||
raise ValueError("Too large value for seconds: %s", seconds)
|
||||
command += pack('<H', msec)
|
||||
|
||||
self._working = -1
|
||||
self._wrap_and_write(command, speed_primary, speed_secondary)
|
||||
self.__wait_sync(async)
|
||||
|
||||
@ -158,6 +158,7 @@ class EncodedMotor(Peripheral):
|
||||
# angle
|
||||
command += pack('<I', angle)
|
||||
|
||||
self._working = -1
|
||||
self._wrap_and_write(command, speed_primary, speed_secondary)
|
||||
self.__wait_sync(async)
|
||||
|
||||
@ -165,7 +166,9 @@ class EncodedMotor(Peripheral):
|
||||
if not async:
|
||||
log.debug("Waiting for sync command work to finish...")
|
||||
while self.is_working():
|
||||
time.sleep(0.05)
|
||||
log.debug("Waiting")
|
||||
time.sleep(0.1)
|
||||
log.debug("Command has finished.")
|
||||
|
||||
# TODO: how to tell when motor has stopped?
|
||||
|
||||
|
||||
17
tests.py
17
tests.py
@ -1,4 +1,5 @@
|
||||
import unittest
|
||||
from binascii import unhexlify
|
||||
|
||||
from pylgbst import *
|
||||
|
||||
@ -33,7 +34,7 @@ class ConnectionMock(Connection):
|
||||
if self.notification_handler:
|
||||
while self.notifications:
|
||||
handle, data = self.notifications.pop(0)
|
||||
self.notification_handler(handle, hex2str(data.replace(' ', '')))
|
||||
self.notification_handler(handle, unhexlify(data.replace(' ', '')))
|
||||
time.sleep(0.1)
|
||||
|
||||
self.finished = True
|
||||
@ -69,7 +70,7 @@ class GeneralTest(unittest.TestCase):
|
||||
hub = HubMock()
|
||||
led = LED(hub, PORT_LED)
|
||||
led.set_color(COLOR_RED)
|
||||
self.assertEqual("0801813211510009", hub.connection.writes[0][1])
|
||||
self.assertEqual("0801813201510009", hub.connection.writes[0][1])
|
||||
|
||||
def test_tilt_sensor(self):
|
||||
hub = HubMock()
|
||||
@ -103,10 +104,14 @@ class GeneralTest(unittest.TestCase):
|
||||
conn = ConnectionMock()
|
||||
conn.notifications.append((14, '1b0e00 0900 04 39 0227003738'))
|
||||
hub = HubMock(conn)
|
||||
motor = EncodedMotor(hub, PORT_AB)
|
||||
motor.timed(1.5)
|
||||
time.sleep(0.1)
|
||||
|
||||
conn.notifications.append((14, '1b0e00050082390a'))
|
||||
hub.motor_AB.timed(1.5)
|
||||
self.assertEqual("0d018139110adc056464647f03", conn.writes[0][1])
|
||||
motor.angled(90)
|
||||
|
||||
conn.notifications.append((14, '1b0e00050082390a'))
|
||||
hub.motor_AB.angled(90)
|
||||
self.assertEqual("0f018139110c5a0000006464647f03", conn.writes[1][1])
|
||||
|
||||
def test_capabilities(self):
|
||||
@ -133,7 +138,7 @@ class GeneralTest(unittest.TestCase):
|
||||
hub.connection.notifications.append((HANDLE, '1b0e000f0004010125000000001000000010'))
|
||||
time.sleep(1)
|
||||
|
||||
def callback(color, unk1, unk2):
|
||||
def callback(color, unk1, unk2=None):
|
||||
name = COLORS[color] if color is not None else 'NONE'
|
||||
log.info("Color: %s %s %s", name, unk1, unk2)
|
||||
|
||||
|
||||
@ -22,6 +22,7 @@ class Vernie(MoveHub):
|
||||
self._color_detected = COLOR_NONE
|
||||
self._sensor_distance = 10
|
||||
self.color_distance_sensor.subscribe(self._color_distance_data)
|
||||
log.info("Vernie is ready.")
|
||||
|
||||
def _external_motor_data(self, data):
|
||||
log.debug("External motor position: %s", data)
|
||||
@ -43,6 +44,10 @@ class Vernie(MoveHub):
|
||||
|
||||
def program(self):
|
||||
time.sleep(1)
|
||||
self.head_to(LEFT, angle=90)
|
||||
self.head_to(RIGHT, angle=50)
|
||||
time.sleep(1)
|
||||
|
||||
while True:
|
||||
self.head_to(LEFT)
|
||||
time.sleep(1)
|
||||
@ -59,6 +64,7 @@ class Vernie(MoveHub):
|
||||
|
||||
if __name__ == '__main__':
|
||||
logging.basicConfig(level=logging.DEBUG)
|
||||
comms.log.setLevel(logging.INFO)
|
||||
|
||||
try:
|
||||
connection = DebugServerConnection()
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user