Ejemplo n.º 1
0
  def send_message(self, msg_type, msg, sender_id=0x42):
    framed_msg = struct.pack('<BHHB', SBP_PREAMBLE, msg_type, sender_id, len(msg))
    framed_msg += msg
    crc = crc16(framed_msg[1:], 0)
    framed_msg += struct.pack('<H', crc)

    self.ser.write(framed_msg)
Ejemplo n.º 2
0
  def get_message(self):
    while True:
      if self.lt.wants_to_stop:
        return SBP(None, None, None, None, None)

      # Sync with magic start bytes
      magic = self.ser.read(1)
      if magic:
        if ord(magic) == SBP_PREAMBLE:
          break
        else:
          self.unhandled_bytes += 1
          if self.print_unhandled:
            print "Host Side Unhandled byte : 0x%02x," % ord(magic), \
                                                            "total", \
                                               self.unhandled_bytes

    hdr = ""
    while len(hdr) < 5:
      hdr = self.ser.read(5 - len(hdr))

    msg_type, sender_id, msg_len = struct.unpack('<HHB', hdr)
    crc = crc16(hdr, 0)

    data = ""
    while len(data) < msg_len:
      data += self.ser.read(msg_len - len(data))
    crc = crc16(data, crc)

    crc_received = ""
    while len(crc_received) < 2:
      crc_received = self.ser.read(2 - len(crc_received))
    crc_received = struct.unpack('<H', crc_received)[0]

    if crc != crc_received:
      print "Host Side CRC mismatch: 0x%04X 0x%04X" % (crc, crc_received)
      return SBP(None, None, None, None, None)

    return SBP(msg_type, sender_id, msg_len, data, crc)