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)
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)
