From 4ab378499e1007c3b12a3722d0c2cc82a21379a8 Mon Sep 17 00:00:00 2001
From: Andrey Pohilko <apc4@ya.ru>
Date: Tue, 19 Sep 2017 16:34:39 +0300
Subject: [PATCH] Found problem of stuck notifications

---
 README.md              |  1 -
 pylgbst/__init__.py    |  4 +++-
 pylgbst/peripherals.py | 36 ++++++++++++++++++++----------------
 3 files changed, 23 insertions(+), 18 deletions(-)

diff --git a/README.md b/README.md
index a26ab74..014d84c 100644
--- a/README.md
+++ b/README.md
@@ -68,7 +68,6 @@ sudo python -c "from pylgbst.comms import *; \
 - document all API methods
 - make sure unit tests cover all important code
 - generalize getting device info + give constants (low priority)
-- subscribing to 2 sensors at once causes port status to not arrive => sync mode stuck. Why?
 - can we subscribe to LED?
 - organize requesting and printing device info on startup - firmware version at least
 - make debug server to re-establish BLE connection on loss
diff --git a/pylgbst/__init__.py b/pylgbst/__init__.py
index 00e1898..5ac375c 100644
--- a/pylgbst/__init__.py
+++ b/pylgbst/__init__.py
@@ -78,7 +78,9 @@ class MoveHub(object):
         elif msg_type == MSG_SENSOR_DATA:
             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)])
+            port = get_byte(data, 3)
+            log.debug("Sensor subscribe ack on port %s", PORTS[port])
+            self.devices[port].finished()
         elif msg_type == MSG_PORT_CMD_ERROR:
             log.warning("Command error: %s", str2hex(data[3:]))
         elif msg_type == MSG_DEVICE_SHUTDOWN:
diff --git a/pylgbst/peripherals.py b/pylgbst/peripherals.py
index 0d56e90..3ab2b9d 100644
--- a/pylgbst/peripherals.py
+++ b/pylgbst/peripherals.py
@@ -60,12 +60,15 @@ class Peripheral(object):
     def in_progress(self):
         return bool(self._working)
 
-    def subscribe(self, callback, mode, granularity=1):
+    def subscribe(self, callback, mode, granularity=1, async=False):
         self._port_subscription_mode = mode
+        self.started()
         self._port_subscribe(self._port_subscription_mode, granularity, True)
         if callback:
             self._subscribers.add(callback)
 
+        self._wait_sync(async)  # having async=True leads to stuck notifications
+
     def unsubscribe(self, callback=None):
         if callback in self._subscribers:
             self._subscribers.remove(callback)
@@ -95,6 +98,13 @@ class Peripheral(object):
             except BaseException:
                 log.warning("Failed to handle port data by %s: %s", self, str2hex(data))
 
+    def _wait_sync(self, async):
+        if not async:
+            log.debug("Waiting for sync command work to finish...")
+            while self.in_progress():
+                time.sleep(0.5)
+            log.debug("Command has finished.")
+
 
 class LED(Peripheral):
     SOMETHING = b'\x51\x00'
@@ -178,7 +188,7 @@ class EncodedMotor(Peripheral):
 
         self.started()
         self._wrap_and_write(command, speed_primary, speed_secondary)
-        self.__wait_sync(async)
+        self._wait_sync(async)
 
     def angled(self, angle, speed_primary=1, speed_secondary=None, async=False):
         if speed_secondary is None:
@@ -196,7 +206,7 @@ class EncodedMotor(Peripheral):
 
         self.started()
         self._wrap_and_write(command, speed_primary, speed_secondary)
-        self.__wait_sync(async)
+        self._wait_sync(async)
 
     def constant(self, speed_primary=1, speed_secondary=None, async=False):
         if speed_secondary is None:
@@ -207,9 +217,9 @@ class EncodedMotor(Peripheral):
 
         self.started()
         self._wrap_and_write(command, speed_primary, speed_secondary)
-        self.__wait_sync(async)
+        self._wait_sync(async)
 
-    def some(self, speed_primary=1, speed_secondary=None, async=False):
+    def __some(self, speed_primary=1, speed_secondary=None, async=False):
         if speed_secondary is None:
             speed_secondary = speed_primary
 
@@ -218,12 +228,12 @@ class EncodedMotor(Peripheral):
 
         self.started()
         self._wrap_and_write(command, speed_primary, speed_secondary)
-        self.__wait_sync(async)
+        self._wait_sync(async)
 
     def stop(self):
         self.constant(0, async=True)
 
-    def test(self, speed_primary=1, speed_secondary=None):
+    def __test(self, speed_primary=1, speed_secondary=None):
         if speed_secondary is None:
             speed_secondary = speed_primary
 
@@ -236,8 +246,8 @@ class EncodedMotor(Peripheral):
         command += pack('<H', self._speed_abs(0.2))
         command += pack('<H', 1000)
 
-        #command += pack('<B', self._speed_abs(speed_primary))  # time or angle - first param
-        #command += pack('<B', self._speed_abs(speed_secondary))  # time or angle - first param
+        # command += pack('<B', self._speed_abs(speed_primary))  # time or angle - first param
+        # command += pack('<B', self._speed_abs(speed_secondary))  # time or angle - first param
 
         speed_primary = 0.5
         speed_secondary = -0.5
@@ -249,13 +259,6 @@ class EncodedMotor(Peripheral):
 
         self._write_to_hub(MSG_SET_PORT_VAL, command)
 
-    def __wait_sync(self, async):
-        if not async:
-            log.debug("Waiting for sync command work to finish...")
-            while self.in_progress():
-                time.sleep(0.5)
-            log.debug("Command has finished.")
-
     def handle_port_data(self, data):
         if self._port_subscription_mode == MOTOR_MODE_ANGLE:
             rotation = unpack("<l", data[4:8])[0]
@@ -368,6 +371,7 @@ class Battery(Peripheral):
     # liion 5v ~= 2100
     def handle_port_data(self, data):
         self.last_value = unpack("<h", data[4:6])[0]
+        log.warning("Battery: %s"), self.last_value
         self._notify_subscribers(self.last_value)