From eeb1891cb6037ece5827e3744145a8241ad3a3dc Mon Sep 17 00:00:00 2001 From: Andrey Pohilko Date: Thu, 14 Sep 2017 16:40:57 +0300 Subject: [PATCH] Deeper tilt sensor support --- demo.py | 8 ++++---- pylgbst/__init__.py | 5 +++++ pylgbst/comms.py | 2 +- pylgbst/constants.py | 11 ++++++++--- pylgbst/peripherals.py | 41 ++++++++++++++++++++++++----------------- tests.py | 6 +++--- 6 files changed, 45 insertions(+), 28 deletions(-) diff --git a/demo.py b/demo.py index 70611e8..806dbaa 100644 --- a/demo.py +++ b/demo.py @@ -26,9 +26,9 @@ def demo_tilt_sensor_precise(movehub): demo_tilt_sensor_simple.cnt = 0 limit = 100 - def callback(param1, param2): + def callback(pitch, roll, yaw): demo_tilt_sensor_simple.cnt += 1 - log.info("Tilt #%s of %s: %s %s", demo_tilt_sensor_simple.cnt, limit, param1, param2) + log.info("Tilt #%s of %s: roll:%s pitch:%s yaw:%s", demo_tilt_sensor_simple.cnt, limit, pitch, roll, yaw) movehub.tilt_sensor.subscribe(callback, mode=TILT_SENSOR_MODE_FULL) while demo_tilt_sensor_simple.cnt < limit: @@ -129,9 +129,9 @@ if __name__ == '__main__': connection = BLEConnection().connect() hub = MoveHub(connection) - #demo_tilt_sensor_precise(hub) + demo_tilt_sensor_precise(hub) - demo_all(hub) + # demo_all(hub) log.info("Sleeping 60s") sleep(60) diff --git a/pylgbst/__init__.py b/pylgbst/__init__.py index d603e87..e006f1e 100644 --- a/pylgbst/__init__.py +++ b/pylgbst/__init__.py @@ -81,6 +81,8 @@ class MoveHub(object): self._handle_sensor_data(data) elif msg_type == MSG_SENSOR_SUBSCRIBE_ACK: log.debug("Sensor subscribe ack on port %s", PORTS[get_byte(data, 3)]) + elif msg_type == MSG_PORT_CMD_ERROR: + log.warning("Command error: %s", str2hex(data[3:])) else: log.warning("Unhandled msg type 0x%x: %s", msg_type, str2hex(orig)) else: @@ -88,6 +90,9 @@ class MoveHub(object): def _handle_sensor_data(self, data): port = get_byte(data, 3) + if port not in self.devices: + log.warning("Notification on port with no device: %s", PORTS[port]) + return sensor = self.devices[port] if isinstance(sensor, TiltSensor): sensor.handle_notification(data) diff --git a/pylgbst/comms.py b/pylgbst/comms.py index 3ddfd3e..0901c9d 100644 --- a/pylgbst/comms.py +++ b/pylgbst/comms.py @@ -48,7 +48,7 @@ else: def int2byte(val): - return bytes(val, ) + return bytes((val,)) # noinspection PyMethodOverriding diff --git a/pylgbst/constants.py b/pylgbst/constants.py index 7bc231e..84d361b 100644 --- a/pylgbst/constants.py +++ b/pylgbst/constants.py @@ -59,10 +59,14 @@ PORTS = { } # PACKET TYPES +MSG_DEVICE_INFO = 0x01 +MSG_PING_RESPONSE = 0x03 MSG_PORT_INFO = 0x04 +MSG_PORT_CMD_ERROR = 0x05 MSG_SET_PORT_VAL = 0x81 MSG_PORT_STATUS = 0x82 MSG_SENSOR_SUBSCRIBE = 0x41 +#MSG_SENSOR_UNSUBSCRIBE = 0x42 MSG_SENSOR_DATA = 0x45 MSG_SENSOR_SUBSCRIBE_ACK = 0x47 @@ -91,10 +95,11 @@ STATUS_CONFLICT = 0x05 STATUS_FINISHED = 0x0a # TILT -TILT_SENSOR_MODE_FULL = 0x00 -TILT_SENSOR_MODE_2AXIS = 0x01 +TILT_SENSOR_MODE_2AXIS_FULL = 0x00 +TILT_SENSOR_MODE_2AXIS_SIMPLE = 0x01 TILT_SENSOR_MODE_BASIC = 0x02 -TILT_SENSOR_MODE_OFF = 0x03 +TILT_SENSOR_MODE_BUMP = 0x03 +TILT_SENSOR_MODE_FULL = 0x04 TILT_HORIZONTAL = 0x00 TILT_UP = 0x01 diff --git a/pylgbst/peripherals.py b/pylgbst/peripherals.py index ab43988..acb9d9b 100644 --- a/pylgbst/peripherals.py +++ b/pylgbst/peripherals.py @@ -126,38 +126,45 @@ class EncodedMotor(Peripheral): class TiltSensor(Peripheral): + TRAILER = b'\x01\x00\x00\x00' + def __init__(self, parent, port): super(TiltSensor, self).__init__(parent, port) - self.mode = TILT_SENSOR_MODE_OFF - - def _switch_mode(self, mode): - self.mode = mode - self._subscribe_on_port(int2byte(mode) + b'\x01\x00\x00\x00\x01') + self.mode = None def subscribe(self, callback, mode=TILT_SENSOR_MODE_BASIC): - if mode not in (TILT_SENSOR_MODE_BASIC, TILT_SENSOR_MODE_2AXIS, TILT_SENSOR_MODE_FULL): - raise ValueError("Wrong tilt sensor mode: 0x%x", mode) - - self._switch_mode(mode) - self._subscribers.add(callback) # TODO: maybe join it into `_subscribe_on_port` + # TODO: check input for valid mode + self.mode = mode + self._subscribe_on_port(int2byte(self.mode) + self.TRAILER + int2byte(1)) + self._subscribers.add(callback) def unsubscribe(self, callback): self._subscribers.remove(callback) if not self._subscribers: - self._switch_mode(TILT_SENSOR_MODE_OFF) + self._subscribe_on_port(int2byte(self.mode) + self.TRAILER + int2byte(0)) def handle_notification(self, data): if self.mode == TILT_SENSOR_MODE_BASIC: - self._notify_subscribers(get_byte(data, 4)) - elif self.mode == TILT_SENSOR_MODE_FULL: + state = get_byte(data, 4) + self._notify_subscribers(state) + elif self.mode == TILT_SENSOR_MODE_2AXIS_SIMPLE: + # TODO: figure out right interpreting of this + state = get_byte(data, 4) + self._notify_subscribers(state) + elif self.mode == TILT_SENSOR_MODE_BUMP: + bump_count = get_byte(data, 4) + self._notify_subscribers(bump_count) + elif self.mode == TILT_SENSOR_MODE_2AXIS_FULL: roll = self._byte2deg(get_byte(data, 4)) pitch = self._byte2deg(get_byte(data, 5)) self._notify_subscribers(roll, pitch) - elif self.mode == TILT_SENSOR_MODE_2AXIS: - # TODO: figure out right interpreting of this - self._notify_subscribers(get_byte(data, 4)) + elif self.mode == TILT_SENSOR_MODE_FULL: + roll = self._byte2deg(get_byte(data, 4)) + pitch = self._byte2deg(get_byte(data, 5)) + yaw = self._byte2deg(get_byte(data, 6)) # did I get the order right? + self._notify_subscribers(roll, pitch, yaw) else: - log.debug("Got tilt sensor data while in finished mode: %s", self.mode) + log.debug("Got tilt sensor data while in unexpected mode: %s", self.mode) def _byte2deg(self, val): if val > 90: diff --git a/tests.py b/tests.py index f3c3eca..b118aca 100644 --- a/tests.py +++ b/tests.py @@ -5,7 +5,7 @@ from threading import Thread from pylgbst import MoveHub, COLOR_RED, LED, EncodedMotor, PORT_AB from pylgbst.comms import Connection, str2hex, hex2str -from pylgbst.constants import PORT_LED, TILT_STATES, TILT_SENSOR_MODE_FULL, TILT_SENSOR_MODE_2AXIS, \ +from pylgbst.constants import PORT_LED, TILT_STATES, TILT_SENSOR_MODE_2AXIS_FULL, TILT_SENSOR_MODE_2AXIS_SIMPLE, \ MOVE_HUB_HARDWARE_HANDLE HANDLE = MOVE_HUB_HARDWARE_HANDLE @@ -92,12 +92,12 @@ class GeneralTest(unittest.TestCase): hub.connection.notifications.append((HANDLE, "1b0e000500453a05")) hub.connection.notifications.append((HANDLE, "1b0e000a00473a010100000001")) time.sleep(1) - hub.tilt_sensor.subscribe(callback, TILT_SENSOR_MODE_2AXIS) + hub.tilt_sensor.subscribe(callback, TILT_SENSOR_MODE_2AXIS_SIMPLE) hub.connection.notifications.append((HANDLE, "1b0e000500453a09")) time.sleep(1) - hub.tilt_sensor.subscribe(callback, TILT_SENSOR_MODE_FULL) + hub.tilt_sensor.subscribe(callback, TILT_SENSOR_MODE_2AXIS_FULL) hub.connection.notifications.append((HANDLE, "1b0e000600453a04fe")) time.sleep(1)