def packets(self):
message_type = self.body[0:2]
body_remaining = self.body[2:] + self.computed_crc_bytes()
packets = []
while len(body_remaining) > 0:
packet = Packet()
packet.pod_address_1 = self.pod_id
packet.sequence = self.start_seq + len(packets) * 2
if len(packets) == 0:
packet.packet_type = Packet.PACKET_TYPE_PDM
packet.pod_address_2 = self.pod_id
packet.byte9 = self.byte9
packet.message_type = message_type
segment_len = min(Packet.MAX_BODY_SEGMENT_LEN,
len(body_remaining))
packet.body = body_remaining[:segment_len]
packet.body_len = len(self.body)
body_remaining = body_remaining[segment_len:]
else:
packet.packet_type = Packet.PACKET_TYPE_CON
segment_len = min(Packet.MAX_CON_BODY_SEGMENT_LEN,
len(body_remaining))
packet.body = body_remaining[:segment_len]
body_remaining = body_remaining[segment_len:]
packets.append(packet)
return packets
def packets(self):
message_type = self.body[0:2]
body_remaining = self.body[2:] + self.computed_crc_bytes()
packets = []
while len(body_remaining) > 0:
packet = Packet()
packet.pod_address_1 = self.pod_id
packet.sequence = self.start_seq + len(packets) * 2
if len(packets) == 0:
packet.packet_type = Packet.PACKET_TYPE_PDM
packet.pod_address_2 = self.pod_id
packet.byte9 = self.byte9
packet.message_type = message_type
segment_len = min(Packet.MAX_BODY_SEGMENT_LEN,len(body_remaining))
packet.body = body_remaining[:segment_len]
packet.body_len = len(self.body)
body_remaining = body_remaining[segment_len:]
else:
packet.packet_type = Packet.PACKET_TYPE_CON
segment_len = min(Packet.MAX_CON_BODY_SEGMENT_LEN,len(body_remaining))
packet.body = body_remaining[:segment_len]
body_remaining = body_remaining[segment_len:]
packets.append(packet)
return packets
def decode_control_msg(raw_packet):
if raw_packet[1] != 0x03:
print("- packet parser: decode_control_msg: raw packet not 0x03 type")
return None
if len(raw_packet) != 10:
print("- packet parser: decode_control_msg: raw packet not 10 bytes",
raw_packet.hex())
return None
unpacked = unpack('<ccccfcc', raw_packet)
pkt = Packet()
pkt.message_type = 0x03
pkt.raw = raw_packet
control_msg = {
'node_id': int.from_bytes(unpacked[2], 'little'),
'type': int.from_bytes(unpacked[3], 'little'),
'u': unpacked[4]
}
crc = ((int.from_bytes(unpacked[5], 'little') & 0xff) << 8) | (
int.from_bytes(unpacked[6], 'little') & 0xff)
pkt.valid = validate_crc(raw_packet, crc)
pkt.parsed = control_msg
return pkt
def decode_state_msg(raw_packet):
if raw_packet[1] != 0x01:
print("- packet parser: decode_state_msg: raw packet not 0x01 type")
return None
if len(raw_packet) != 24:
print("- packet parser: decode_state_msg: raw packet not 24 bytes",
raw_packet.hex())
return None
unpacked = unpack('<ccfffffcc', raw_packet)
pkt = Packet()
pkt.message_type = 0x01
pkt.raw = raw_packet
state_msg = {
'x': unpacked[2],
'y': unpacked[3],
'phi': unpacked[4],
'sp_x': unpacked[5],
'sp_y': unpacked[6]
}
crc = ((int.from_bytes(unpacked[7], 'little') & 0xff) << 8) | (
int.from_bytes(unpacked[8], 'little') & 0xff)
pkt.valid = validate_crc(raw_packet, crc)
pkt.parsed = state_msg
return pkt
def decode_user_msg(raw_packet):
if raw_packet[1] != 0x04:
print("- packet parser: decode_user_msg: raw packet not 0x04 type")
return None
pkt = Packet()
pkt.message_type = 0x04
pkt.raw = raw_packet
crc = ((raw_packet[-2] & 0xff) << 8) | (raw_packet[-1] & 0xff)
pkt.valid = validate_crc(raw_packet, crc)
return pkt
def decode_staterq_msg(raw_packet):
if raw_packet[1] != 0x02:
print("- packet parser: decode_staterq_msg: raw packet not 0x02 type")
return None
if len(raw_packet) != 5:
print("- packet parser: decode_staterq_msg: raw packet not 5 bytes",
raw_packet.hex())
return None
unpacked = unpack('<ccccc', raw_packet)
pkt = Packet()
pkt.message_type = 0x02
pkt.raw = raw_packet
staterq_msg = {'node_id': int.from_bytes(unpacked[2], 'little')}
crc = ((int.from_bytes(unpacked[3], 'little') & 0xff) << 8) | (
int.from_bytes(unpacked[4], 'little') & 0xff)
pkt.valid = validate_crc(raw_packet, crc)
pkt.parsed = staterq_msg
return pkt
