Working on tilt sensors

demo.py

@@ -98,7 +98,9 @@ if __name__ == '__main__':
         connection = BLEConnection().connect()
 
     hub = MoveHub(connection)
-    demo_all(hub)
+    hub.tilt_sensor.subscribe(lambda: log.info("Tilt"))
+    sleep(60)
+    # demo_all(hub)
 
     # sleep(1)
     # hub.get_name()

pylgbst/__init__.py

@@ -40,11 +40,11 @@ class MoveHub(object):
         self.port_C = None
         self.port_D = None
 
+        self.connection.set_notify_handler(self._notify)
+
         self._wait_for_devices()
 
     def _wait_for_devices(self):
-        # enables notifications reading
-        self.connection.set_notify_handler(self._notify)
         self.connection.write(ENABLE_NOTIFICATIONS_HANDLE, ENABLE_NOTIFICATIONS_VALUE)
 
         builtin_devices = ()
@@ -66,6 +66,8 @@ class MoveHub(object):
         Using https://github.com/JorgePe/BOOSTreveng/blob/master/Notifications.md
         """
         orig = data
+
+        if handle == MOVE_HUB_HARDWARE_HANDLE:
             data = data[3:]
             log.debug("Notification on %s: %s", handle, str2hex(orig))
 
@@ -75,10 +77,18 @@ class MoveHub(object):
                 self._handle_port_info(data)
             elif msg_type == MSG_PORT_STATUS:
                 self._handle_port_status(data)
+            elif msg_type == MSG_SENSOR_DATA:
+                self._handle_sensor_data(data)
             else:
                 log.warning("Unhandled msg type 0x%x: %s", msg_type, str2hex(orig))
+        else:
+            log.warning("Unsupported notification handle: 0x%s", handle)
 
-        pass
+    def _handle_sensor_data(self, data):
+        port = get_byte(data, 3)
+        sensor = self.devices[port]
+        if isinstance(sensor, TiltSensor):
+            sensor.notify_subscribers()
 
     def _handle_port_status(self, data):
         port = get_byte(data, 3)

pylgbst/comms.py

@@ -151,6 +151,8 @@ class DebugServer(object):
             self.ble.requester.notification_sink = lambda x, y: self._notify(conn, x, y)
             try:
                 self._handle_conn(conn)
+            except KeyboardInterrupt:
+                raise
             except BaseException:
                 log.error("Problem handling incoming connection: %s", traceback.format_exc())
             finally:
@@ -165,6 +167,8 @@ class DebugServer(object):
         log.debug("Send notification: %s", payload)
         try:
             conn.send(json.dumps(payload) + "\n")
+        except KeyboardInterrupt:
+            raise
         except BaseException:
             log.error("Problem sending notification: %s", traceback.format_exc())
 
@@ -189,6 +193,8 @@ class DebugServer(object):
                     log.info("Cmd line: %s", line)
                     try:
                         self._handle_cmd(json.loads(line))
+                    except KeyboardInterrupt:
+                        raise
                     except BaseException:
                         log.error("Failed to handle cmd: %s", traceback.format_exc())
 

pylgbst/constants.py

@@ -60,9 +60,10 @@ PORTS = {
 
 # PACKET TYPES
 MSG_PORT_INFO = 0x04
-MSG_PORT_STATUS = 0x82
 MSG_SET_PORT_VAL = 0x81
-MSG_PORT_SUBSCRIBE = 0x41
+MSG_PORT_STATUS = 0x82
+MSG_SENSOR_SUBSCRIBE = 0x41
+MSG_SENSOR_DATA = 0x45
 
 # NOTIFICATIONS
 TYPE_DISTANCE_COLOR_SENSOR = 0x25

pylgbst/peripherals.py

@@ -34,7 +34,7 @@ class Peripheral(object):
 
     def _subscribe_on_port(self, params):
         # FIXME: became obsolete
-        self._write_to_hub(MSG_PORT_SUBSCRIBE, params)
+        self._write_to_hub(MSG_SENSOR_SUBSCRIBE, params)
 
     def started(self):
         self.working = True
@@ -42,6 +42,10 @@ class Peripheral(object):
     def finished(self):
         self.working = False
 
+    def notify_subscribers(self, *args, **kwargs):
+        for subscriber in self._subscribers:
+            subscriber(*args, **kwargs)
+
 
 class LED(Peripheral):
     def set_color(self, color):
@@ -122,8 +126,8 @@ class ColorDistanceSensor(Peripheral):
 
 class TiltSensor(Peripheral):
     def subscribe(self, callback):
-        params = b'\x00\x01\x00\x00\x00\x01'  # full
+        #params = b'\x00\x01\x00\x00\x00\x01'  # full
-        # params = b'\x02\x01\x00\x00\x00\x01'  # basic
+        params = b'\x02\x01\x00\x00\x00\x01'  # basic
         self._subscribe_on_port(params)
         self._subscribers.append(callback)  # TODO: maybe join it into `_subscribe_on_port`
 

tests.py

@@ -5,8 +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, PORT_TILT_SENSOR
+from pylgbst.constants import PORT_LED
-from pylgbst.peripherals import TiltSensor
 
 logging.basicConfig(level=logging.DEBUG)
 
@@ -60,6 +59,13 @@ class HubMock(MoveHub):
 
 
 class GeneralTest(unittest.TestCase):
+    def _wait_notifications_handled(self, hub):
+        hub.connection.running = False
+        for _ in range(1, 1000):
+            time.sleep(0.1)
+            if hub.connection.finished:
+                break
+
     def test_led(self):
         hub = HubMock()
         led = LED(hub, PORT_LED)
@@ -68,13 +74,17 @@ class GeneralTest(unittest.TestCase):
 
     def test_tilt_sensor(self):
         hub = HubMock()
-        hub.tilt_sensor = TiltSensor(hub, PORT_TILT_SENSOR)
+        hub.connection.notifications.append((14, '1b0e00 0f00 04 3a 0128000000000100000001'))
+        time.sleep(1)
 
         def callback():
-            pass
+            log.debug("Tilt")
 
         hub.tilt_sensor.subscribe(callback)
+        hub.connection.notifications.append((14, "1b0e000600453a04fe"))
+        time.sleep(10)
         self.assertEquals("0a01413a000100000001", hub.connection.writes[0][1])
+        self._wait_notifications_handled(hub)
 
     def test_motor(self):
         conn = ConnectionMock()
@@ -102,7 +112,4 @@ class GeneralTest(unittest.TestCase):
 
         hub = MoveHub(conn)
         # demo_all(hub)
-        conn.running = False
+        self._wait_notifications_handled(hub)
-
-        while not conn.finished:
-            time.sleep(0.1)