added GSV4 serial protocol

software/wirelessLoadCell/GSV4BT.py

@@ -2,18 +2,58 @@ import sys
 import bluetooth
 
 class GSV4BT():
+  # https://www.manualslib.com/manual/1380505/Me-Gsv-4.html?page=30#manual
+  scalings = {
+    # (ID, value for 0xFFFF, unit)
+    '2mV':    (0x01, 2.1 , 'mV/V'),  # Measuring range ±2 mV/V (set_gain 0xB2 <p1> <p2>) with p1=ch, p2=0x01
+    '10mV':   (0x02, 10.5, 'mV/V'),  # Measuring range ±10 mV/V (set_gain 0xB2 <p1> <p2>) with p1=ch, p2=0x02
+    '5V':     (0x03, 5.25, 'V'   ),  # Measuring range 0-5 V (set_gain 0xB2 <p1> <p2>) with p1=ch, p2=0x03
+    '10V':    (0x07, 10.5, 'V'   ),  # Measuring range 0-10 V (set_gain 0xB2 <p1> <p2>) with p1=ch, p2=0x07
+    'PT1000': (0x04, 1050, '°C'  ),  # Measuring range PT1000 (set_gain 0xB2 <p1> <p2>) with p1=ch, p2=0x04
+    'K':      (0x06, 1050, '°C'  )   # Measuring range K-thermocouple cable (set_gain 0xB2 <p1> <p2>) with p1=ch, p2=0x06
+  }
+  channelModes = [
+    None,
+    None,
+    None,
+    None
+  ]
+
+  frequencies = [
+    # (ID, fNom in Hz, fEff in Hz)
+    (0xA0, 0.63, 0.625),
+    (0xA1, 1.25, 1.250),
+    (0xA2, 2.5 , 2.500),
+    (0xA3, 3.75, 3.750),
+    (0xA4, 6.25, 6.250),
+    (0xA5, 7.5 , 7.500),
+    (0xA6, 12.5, 12.40),
+    (0xA7, 15  , 14.7 ),
+    (0xA8, 25  , 24.4 ),
+    (0xA9, 125 , 125  ),
+    (0xAA, 250 , 250  ),
+    (0xAB, 500 , 500  ),
+    (0xAC, 937.5, 900 ),
+  ]
+
   def __init__(self, addr):
     self.addr = addr
     self.uuid = None
     self.sock = None
 
+  def printError(self, msg):
+    print("ERROR:   GSV4BT {}:".format(self.addr), msg)
+
+  def printWarning(self, msg):
+    print("WARNING: GSV4BT {}:".format(self.addr), msg)
+
   def connect(self):
     if self.sock:
       return True
 
     service_matches = bluetooth.find_service(address=self.addr)
     if len(service_matches) == 0:
-      print("BT device {} not found.".format(self.addr))
+      self.printError("BT device not found.")
       return False
     
     first_match = service_matches[0]
@@ -22,19 +62,171 @@ class GSV4BT():
     self.name = first_match["name"]
     self.host = first_match["host"]
 
-    print("Connecting to \"{}\" on {}".format(self.name, self.host))
+    self.printWarning("Connecting to \"{}\" on {}".format(self.name, self.host))
 
     self.sock = bluetooth.BluetoothSocket(bluetooth.RFCOMM)
-    return self.sock.connect((self.host, self.port))
+    ret = self.sock.connect((self.host, self.port))
+    self.sock.settimeout(0.3)
+    return ret
+  
+  def isConnected(self):
+    return self.sock and True
   
   def sendRaw(self, data):
     self.sock.send(data)
 
+  def sendCommand(self, code, *params):
+    data = bytes([code] + params)
+    return self.sendRaw(data)
+
+  def setNormalMode(self):
+    return self.sendRaw(bytes(0x26, 0x01, 0x62, 0x65, 0x72, 0x6C, 0x69, 0x6E))
+
+  def getMode(self):
+    self.sendCommand(0x27)
+    return self.waitResponse(0x27)
+
+  def getFirmwareVersion(self):
+    self.sendCommand(0x2B)
+    return self.waitResponse(0x2B)
+  
+  def getTxStatus(self):
+    self.sendCommand(0x29)
+    return self.waitResponse(0x29)
+
+  # https://www.manualslib.com/manual/1380505/Me-Gsv-4.html?page=34#manual
+  def setGain(self, channel, scalingName = '2mV'):
+    self.channelModes[channel] = scalingName
+    self.sendCommand(0xB2, channel, self.scalings[scalingName][0])
+    return self.waitResponse(0xB2)
+
+  def setFrequency(self, freq = 10):
+    for id, fNom, fEff in self.frequencies:
+      value = id
+      if freq < fEff:
+        break
+    self.sendCommand(0x12, value)
+    return self.waitResponse(0x12)
+
+  def startTransmission(self):
+    self.sendCommand(0x24)
+    return self.waitResponse(0x24)
+
+  def stopTransmission(self):
+    self.sendCommand(0x23)
+    return self.waitResponse(0x23)
+
+  def getValue(self):
+    self.sendCommand(0x3B)
+
+  lastFrameCarry = None
   def recvRaw(self):
-    return self.sock.recv(1024)
+    data = self.sock.recv(1024)
+    if self.lastFrameCarry:
+      data = self.lastFrameCarry + data
+
+    i = 0
+    while i < len(data):
+      prefix = data[i]
+      start = i
+      i += 1
+      end = data.find(b'\r\n', start)
+
+      if end == -1:
+        self.lastFrameCarry = data[start:]
+        break
+      else:
+        i = end + 2
+
+      if prefix == 0xA5:
+        # measured values
+        if end+2 - start != 11:
+          self.printError("invalid frame: values", 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])
+          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)
+        if end+2 - start != length + 10:
+          self.printError("invalid length: {}", data[start:end+2])
+          continue
+        self.parseResponse(code, n, no, data[start+8:end])
+
+  _valuesCb = None
+
+  def setValuesCb(self, cb):
+    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)
+      # 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))
+    if self._valuesCb:
+      _valuesCb(values)
+
+  _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.recvRaw()
+    if recvCode != sentCode:
+      self.printError("invalid response code:", recvCode)
+      return None
+    self.setResponseCb(userRespCb)
+    return recvData
+
+  _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:
+      self.printWarning("more than 1 response:", n-1)
+
+    if self._responseCb:
+      self._responseCb(code, data)
 
   def getForces(self):
     return (0, 0, 0)
 
   def close(self):
-    self.sock.close()
+    self.sock.close()
+
+if __name__ == "__main__":
+  cell = GSV4BT("01:23:45:67:89:01")
+  while True:
+    if cell.isConnected():
+      cell.recvRaw()
+    else:
+      cell.connect()
+      cell.setNormalMode()
+      cell.setFrequency(20) # Hz
+      cell.setGain(0, '2mV')
+      cell.setGain(1, '2mV')
+      cell.setGain(2, '2mV')
+      cell.setGain(3, '5V')
+      cell.setValuesCb(lambda values: print(values))
+    time.sleep(0.3)