GSV4BT bugfixes

software/wirelessLoadCell/GSV4BT.py

@@ -1,5 +1,9 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+
 import sys
 import bluetooth
+import time
 
 class GSV4BT():
   # https://www.manualslib.com/manual/1380505/Me-Gsv-4.html?page=30#manual
@@ -19,6 +23,8 @@ class GSV4BT():
     None
   ]
 
+  values = [0] * 4
+
   frequencies = [
     # (ID, fNom in Hz, fEff in Hz)
     (0xA0, 0.63, 0.625),
@@ -40,47 +46,56 @@ class GSV4BT():
     self.addr = addr
     self.uuid = None
     self.sock = None
+    self.requiresSetup = True
 
-  def printError(self, msg):
-    print("ERROR:   GSV4BT {}:".format(self.addr), msg)
+  def printError(self, msg, var = ""):
+    print("ERROR:   GSV4BT {}:".format(self.addr), msg, var)
 
-  def printWarning(self, msg):
-    print("WARNING: GSV4BT {}:".format(self.addr), msg)
+  def printWarning(self, msg, var = ""):
+    print("WARNING: GSV4BT {}:".format(self.addr), msg, var)
 
   def connect(self):
     if self.sock:
       return True
-
-    service_matches = bluetooth.find_service(address=self.addr)
+    
+    service_matches = bluetooth.find_service(address=self.addr, uuid=bluetooth.SERIAL_PORT_CLASS)
     if len(service_matches) == 0:
       self.printError("BT device not found.")
       return False
     
     first_match = service_matches[0]
-    self.uuid = first_match["uuid"]
     self.port = first_match["port"]
     self.name = first_match["name"]
     self.host = first_match["host"]
 
-    self.printWarning("Connecting to \"{}\" on {}".format(self.name, self.host))
+    self.printWarning("Connecting to \"{}\" on {} port {}".format(self.name, self.host, self.port))
 
     self.sock = bluetooth.BluetoothSocket(bluetooth.RFCOMM)
-    ret = self.sock.connect((self.host, self.port))
+    try:
+      ret = self.sock.connect((self.addr, self.port))
+    except bluetooth.btcommon.BluetoothError as e:
+      self.printError(e)
+      return False
+
     self.sock.settimeout(0.3)
-    return ret
+    return True
   
   def isConnected(self):
     return self.sock and True
   
   def sendRaw(self, data):
-    self.sock.send(data)
+    if self.sock:
+      try:
+        self.sock.send(data)
+      except bluetooth.btcommon.BluetoothError as e:
+        self.printError("send", e)
 
   def sendCommand(self, code, *params):
-    data = bytes([code] + params)
+    data = bytes([code] + list(params))
     return self.sendRaw(data)
 
   def setNormalMode(self):
-    return self.sendRaw(bytes(0x26, 0x01, 0x62, 0x65, 0x72, 0x6C, 0x69, 0x6E))
+    return self.sendRaw(bytes([0x26, 0x01, 0x62, 0x65, 0x72, 0x6C, 0x69, 0x6E]))
 
   def getMode(self):
     self.sendCommand(0x27)
@@ -121,9 +136,20 @@ class GSV4BT():
 
   lastFrameCarry = None
   def recvRaw(self):
-    data = self.sock.recv(1024)
+    if not self.sock:
+      return
+    data = None
+    try:
+      data = self.sock.recv(1024)
+    except bluetooth.btcommon.BluetoothError as e:
+      self.printError("receive", e)
+
+    if data == None:
+      return
+
     if self.lastFrameCarry:
       data = self.lastFrameCarry + data
+      self.lastFrameCarry = None
 
     i = 0
     while i < len(data):
@@ -141,21 +167,21 @@ class GSV4BT():
       if prefix == 0xA5:
         # measured values
         if end+2 - start != 11:
-          self.printError("invalid frame: values", data[start:end+2])
+          self.printError("invalid values frame: length {}".format(end+2-start), data[start:end+2])
           continue
         self.parseValues(data[start+1:end])
       elif prefix == 0x3B:
         # response
         if end+2 - start < 10:
-          self.printError("invalid frame: response", data[start:end+2])
+          self.printError("invalid response frame: length {}".format(end+2-start), data[start:end+2])
           continue
 
         code   = int(data[start+1])
         n      = int(data[start+2])
-        length = int.from_bytes(data[start+3:start+5], 'little', False)
-        no     = int.from_bytes(data[start+5:start+8], 'little', False)
+        length = int.from_bytes(data[start+3:start+5], 'big', signed=False)
+        no     = data[start+5:start+8].decode('ascii')
         if end+2 - start != length + 10:
-          self.printError("invalid length: {}", data[start:end+2])
+          self.printError("incorrect response length field: {}".format(length), data[start:end+2])
           continue
         self.parseResponse(code, n, no, data[start+8:end])
 
@@ -165,58 +191,58 @@ class GSV4BT():
     self._valuesCb = cb
 
   def parseValues(self, data):
-    values = [None]*4
     for i in range(4):
-      values[i] = int.from_bytes(data[i*2:i*2+2], 'little', False)
+      self.values[i] = int.from_bytes(data[i*2:i*2+2], 'big', signed=False)
       # map range to units
       if self.channelModes[i] == None:
         continue
       _, scale, unit = self.scalings[self.channelModes[i]]
-      values[i] = (values[i] - 32768) / 32768 * scale
-      if not self._valuesCb:
-        self.printWarning("missed reading: ch {}: {:8.3f} {}".format(i, values[i], unit))
+      self.values[i] = (self.values[i] - 32768) / 32768 * scale
     if self._valuesCb:
-      _valuesCb(values)
+      self._valuesCb(self.values)
 
-  _responseCb = None
+  _respCode = None
+  _respData = None
+  def _responseCbWait(self, code, data):
+    self._respCode = code
+    self._respData = data
 
+  _responseCb = None
   def setResponseCb(self, cb):
     self._responseCb = cb
 
   def waitResponse(self, sentCode):
     userRespCb = self._responseCb
-    recvCode = None
-    recvData = None
-    def cb(code, data):
-      recvCode = code
-      recvData = data
-    self.setResponseCb(cb)
+    self._respCode = None
+    self._respData = None
+    self.setResponseCb(self._responseCbWait)
     self.recvRaw()
-    if recvCode != sentCode:
-      self.printError("invalid response code:", recvCode)
+    if self._respCode == None:
+      return None
+    if self._respCode != sentCode:
+      self.printError("invalid response code:", hex(self._respCode))
       return None
     self.setResponseCb(userRespCb)
-    return recvData
+    return self._respData
 
-  _responseNo = 0
   def parseResponse(self, code, n, no, data):
-    if no != (self._responseNo + 1) % 2**24:
-      self.printWarning("responses skipped:", (no-self._responseNo+2**24-1) % 2**24)
-    self._responseNo = no
-    if n > 0:
+    if n > 1:
       self.printWarning("more than 1 response:", n-1)
 
     if self._responseCb:
       self._responseCb(code, data)
 
   def getForces(self):
-    return (0, 0, 0)
+    self.recvRaw()
+    return self.values[0:3]
 
   def close(self):
     self.sock.close()
 
 if __name__ == "__main__":
-  cell = GSV4BT("01:23:45:67:89:01")
+  cell = GSV4BT("00:0B:CE:04:F6:66")
+  def cb(values):
+    print(values)
   while True:
     if cell.isConnected():
       cell.recvRaw()
@@ -228,5 +254,6 @@ if __name__ == "__main__":
       cell.setGain(1, '2mV')
       cell.setGain(2, '2mV')
       cell.setGain(3, '5V')
-      cell.setValuesCb(lambda values: print(values))
+      cell.setValuesCb(cb)
+      print(cell.getMode())
     time.sleep(0.3)

software/wirelessLoadCell/loadCells.py

@@ -1,41 +1,69 @@
-from .GSV4BT import GSV4BT
+from GSV4BT import GSV4BT
 import time
 import bluetooth
+import threading
 
 class LoadCells():
   def __init__(self):
     self.cells = (
-      GSV4BT("addr1"),
-      GSV4BT("addr2"),
-      GSV4BT("addr3"),
+      GSV4BT("00:0B:CE:04:F6:66"),
+      GSV4BT("00:0B:CE:04:F6:67"),
+      GSV4BT("00:0B:CE:04:F6:68"),
     )
   
   def connect(self):
     success = True
     for cell in self.cells:
-      if not cell.connect():
+      if cell.isConnected():
+        pass
+      elif cell.connect():
+        cell.requiresSetup = True
+      else:
         success = False
     return success
 
-  def getForces(self):
-    return (
-      self.cells[0].getForces(),
-      self.cells[1].getForces(),
-      self.cells[2].getForces()
-    )
+  def reconnectThread(self):
+    while True:
+      self.connect()
+      time.sleep(1)
 
+  def start(self):
+    self.thread = threading.Thread(target=self.reconnectThread)
+    self.thread.start()
 
-if __name__ == "__main__":
-  cells = LoadCells()
+  def getForces(self, id):
+    cell = self.cells[id]
+    if not cell.isConnected():
+      return None
+    if cell.requiresSetup:
+      cell.setNormalMode()
+      cell.stopTransmission()
+      cell.setFrequency(20) # Hz
+      cell.setGain(0, '2mV')
+      cell.setGain(1, '2mV')
+      cell.setGain(2, '2mV')
+      mode = cell.getMode()
+      if mode and mode[0] == 0x01:
+        cell.requiresSetup = False
 
-  while True:
+    cell.getValue()
+    return cell.getForces()
+
+  def scan(self):
     nearby_devices = bluetooth.discover_devices(lookup_names=True)
     print("Found {} devices.".format(len(nearby_devices)))
 
     for addr, name in nearby_devices:
       print("  {} - {}".format(addr, name))
 
-    cells.connect()
-    print(cells.getForces())
-    cells.scan()
-    time.sleep(1)
+if __name__ == "__main__":
+  cells = LoadCells()
+
+  cells.scan()
+  cells.start()
+  while True:
+
+    vals = cells.getForces(0)
+    if vals:
+      print('cell 0: ' + ' '.join(["ch {}: {:8.3f} mV/V".format(i, vals[i]) for i in range(len(vals))]))
+    time.sleep(.3)