From f7e359af6d04702b6c6e8c4acae8d110eba0ed6e Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Odd=20Str=C3=A5b=C3=B8?= <oddstr13@openshell.no>
Date: Sat, 20 Nov 2021 09:00:28 +0100
Subject: [PATCH] Improve read reliability

---
 solar_ble.py | 60 ++++++++++++++++++++++++++++++++++++++++++++++------
 1 file changed, 53 insertions(+), 7 deletions(-)

diff --git a/solar_ble.py b/solar_ble.py
index 234624b..3f4ccf4 100755
--- a/solar_ble.py
+++ b/solar_ble.py
@@ -230,8 +230,8 @@ def construct_request(address, words=1, action=ACTION_READ, marker=0xFF):
     return struct.pack("!BBHH", marker, action, address, words)
 
 
-def log(*message: object):
-    print(datetime.datetime.utcnow().isoformat(" "), *message)
+def log(*message: object, **kwargs):
+    print(datetime.datetime.utcnow().isoformat(" "), *message, **kwargs)
     sys.stdout.flush()
 
 
@@ -255,12 +255,52 @@ def parse_packet(data):
     return payload
 
 
-def readMemory(fh: RawIOBase, address: int, words: int = 1):
+def discardUntil(fh: RawIOBase, byte: int, timeout=10) -> Optional[int]:
+    assert byte >= 0 and byte < 256, f"byte: Expected 8bit unsigned int, got {byte}"
+
+    def expand(b: Optional[bytes]):
+        if b is None:
+            return b
+        return b[0]
+
+    start = time.time()
+    discarded = 0
+    read_byte = expand(fh.read(1))
+    while read_byte != byte:
+
+        if read_byte is not None:
+            if not discarded:
+                log("Discarding", end="")
+            discarded += 1
+            print(f" {read_byte:02X}", end="")
+            sys.stdout.flush()
+
+        if time.time() - start > timeout:
+            read_byte = None
+            break
+
+        read_byte = expand(fh.read(1))
+
+    if discarded:
+        print()
+        sys.stdout.flush()
+
+    return read_byte
+
+
+def readMemory(fh: RawIOBase, address: int, words: int = 1) -> Optional[bytes]:
     # log(f"Reading {words} words from 0x{address:04X}")
-    write(fh, construct_request(address, words=words))
-    header = fh.read(3)
-    if header and len(header) == 3:
-        tag, operation, size = header
+    request = construct_request(address, words=words)
+    # log("Request:", request)
+    write(fh, request)
+
+    tag = discardUntil(fh, 0xFF)
+    if tag is None:
+        return None
+
+    header = fh.read(2)
+    if header and len(header) == 2:
+        operation, size = header
         data = fh.read(size)
         _crc = fh.read(2)
         if data and _crc:
@@ -268,6 +308,10 @@ def readMemory(fh: RawIOBase, address: int, words: int = 1):
             calculated_crc = modbus(bytes([tag, operation, size, *data]))
             if crc == calculated_crc:
                 return data
+            else:
+                log(f"readMemory: CRC error; {crc:04X} != {calculated_crc:04X}")
+        log("data or crc is falsely", header, data, _crc)
+    return None
 
 
 class Periodical:
@@ -311,6 +355,8 @@ def try_read_parse(
                 log(e)
                 log("0x0100 Unpack error:", len(res), res)
                 log("Flushed from read buffer; ", dev.read(timeout=0.5))
+        else:
+            log(f"No data read, expected {words*2} bytes (attempts left: {attempts})")
     return None