windkanal/software/wirelessLoadCell/GSV4BT.py

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:
      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.sock = bluetooth.BluetoothSocket(bluetooth.RFCOMM)
    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):
    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()

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)