def convert_to_bytes(msg):
"""
Re-builds the MAVLink byte stream from mavros_msgs/Mavlink messages.
"""
payload_octets = len(msg.payload64)
msg_len = 6 + msg.len # header + payload length
if payload_octets < msg.len / 8:
raise ValueError(
"Specified payload length is bigger than actual payload64")
msgdata = bytearray(
struct.pack('<BBBBBB%dQ' % payload_octets, 254, msg.len, msg.seq,
msg.sysid, msg.compid, msg.msgid, *msg.payload64))
if payload_octets != msg.len / 8:
# message is shorter than payload octets
msgdata = msgdata[:msg_len]
if hasattr(mag, 'checksum'):
# since #286 Mavlink.msg save original checksum, so recalculation not needed.
crc16 = msg.checksum
else:
# from MAVLink.decode()
message_type = mavutil.mavlink.mavlink_map[msg.msgid]
crc_extra = message_type.crc_extra
# calculate crc16
crcbuf = msgdata[1:]
crcbuf.append(crc_extra)
crc16 = x25crc(crcbuf).crc
# finalize
msgdata += struct.pack('<H', crc16)
return msgdata
def convert_to_bytes(msg):
"""
Re-builds the MAVLink byte stream from mavros_msgs/Mavlink messages.
"""
payload_octets = len(msg.payload64)
msg_len = 6 + msg.len # header + payload length
if payload_octets < msg.len / 8:
raise ValueError("Specified payload length is bigger than actual payload64")
msgdata = bytearray(
struct.pack(
'<BBBBBB%dQ' % payload_octets,
254, msg.len, msg.seq, msg.sysid, msg.compid, msg.msgid,
*msg.payload64))
if payload_octets != msg.len / 8:
# message is shorter than payload octets
msgdata = msgdata[:msg_len]
# from MAVLink.decode()
message_type = mavutil.mavlink.mavlink_map[msg.msgid]
crc_extra = message_type.crc_extra
# calculate crc16
crcbuf = msgdata[1:]
crcbuf.append(crc_extra)
crc16 = x25crc(crcbuf).crc
# finalize
msgdata += struct.pack('<H', crc16)
return msgdata
def replace_seq(msg, seq):
"""
from https://mavlink.io/en/about/overview.html
uint8_t magic; ///< protocol magic marker
uint8_t len; ///< Length of payload
uint8_t incompat_flags; ///< flags that must be understood
uint8_t compat_flags; ///< flags that can be ignored if not understood
uint8_t seq; ///< Sequence of packet
uint8_t sysid; ///< ID of message sender system/aircraft
uint8_t compid; ///< ID of the message sender component
uint8_t msgid 0:7; ///< first 8 bits of the ID of the message
uint8_t msgid 8:15; ///< middle 8 bits of the ID of the message
uint8_t msgid 16:23; ///< last 8 bits of the ID of the message
uint8_t payload[max 255]; ///< A maximum of 255 payload bytes
uint16_t checksum; ///< X.25 CRC
:param msg: MAVLink message from PX4
:param seq: New sequence value
:return: bytes object for the message buffer
"""
data = msg.get_msgbuf()
data[MAVLINK_SEQ_BYTE] = seq % BYTE_MAX
cc = x25crc(bytes(data)[1:-2])
if msg.crc_extra:
cc.accumulate(struct.pack('B', msg.crc_extra))
data[-2], data[-1] = cc.crc & BYTE_MAX, cc.crc >> 8
return bytes(data)
def convert_to_bytes(msg):
"""
This function build wire byte stream from mavros_msgs/Mavlink
"""
pay_len = len(msg.payload64)
last_len = 0
if pay_len < msg.len / 8:
raise ValueError("Message length are bigger than payload64")
if pay_len != msg.len / 8:
# message are shorter than payload quads
pay_len = msg.len / 8
last_len = msg.len % 8
msgdata = bytearray(
struct.pack(
'<BBBBBB%dQ' % pay_len,
254, msg.len, msg.seq, msg.sysid, msg.compid, msg.msgid,
*(msg.payload64[:pay_len])))
if last_len:
q = struct.unpack('8B', struct.pack('<Q', msg.payload64[pay_len]))
msgdata += bytearray(q[:last_len])
# from MAVLink.decode()
type = mavutil.mavlink.mavlink_map[msg.msgid]
crc_extra = type.crc_extra
# calculate crc16
crcbuf = msgdata[1:]
crcbuf.append(crc_extra)
crc16 = x25crc(crcbuf).crc
# finalize
msgdata += struct.pack('<H', crc16)
return msgdata
def convert_to_bytes(msg):
"""
This function build wire byte stream from mavros_msgs/Mavlink
"""
pay_len = len(msg.payload64)
last_len = 0
if pay_len < msg.len / 8:
raise ValueError("Message length are bigger than payload64")
if pay_len != msg.len / 8:
# message are shorter than payload quads
pay_len = msg.len / 8
last_len = msg.len % 8
msgdata = bytearray(
struct.pack('<BBBBBB%dQ' % pay_len, 254, msg.len, msg.seq, msg.sysid,
msg.compid, msg.msgid, *(msg.payload64[:pay_len])))
if last_len:
q = struct.unpack('8B', struct.pack('<Q', msg.payload64[pay_len]))
msgdata += bytearray(q[:last_len])
# from MAVLink.decode()
type = mavutil.mavlink.mavlink_map[msg.msgid]
crc_extra = type.crc_extra
# calculate crc16
crcbuf = msgdata[1:]
crcbuf.append(crc_extra)
crc16 = x25crc(crcbuf).crc
# finalize
msgdata += struct.pack('<H', crc16)
return msgdata
def __init__(self, addr=0, data=[], length=0, retries=3):
self.addr = addr
self.data = data
self.length = length
self.checksum = x25crc((bytearray(data)))
self.messageCounter = 0
self.ack = 0
self.retries = retries
def __init__(self, addr=0, data=[], length=0, retries = 3):
self.addr=addr
self.data=data
self.length=length
self.checksum = x25crc((bytearray(data)))
self.messageCounter=0
self.ack=0
self.retries = retries
def _decode(self, msgbuf):
'''
Decode a buffer as a MAVLink message
'''
try:
magic, mlen, seq, srcSystem, srcComponent, msgId = struct.unpack('cBBBBB', msgbuf[:6])
except struct.error as emsg:
raise Exception('Unable to unpack MAVLink header: %s' % emsg)
if ord(magic) != 254:
raise Exception("invalid MAVLink prefix '%s'" % magic)
if mlen != len(msgbuf)-8:
raise Exception('invalid MAVLink message length. Got %u expected %u, msgId=%u' % (len(msgbuf) - 8, mlen, msgId))
if not msgId in mavlink.mavlink_map:
raise Exception('unknown MAVLink message ID %u' % msgId)
# decode the payload
type = mavlink.mavlink_map[msgId]
fmt = type.format
order_map = type.orders
len_map = type.lengths
crc_extra = type.crc_extra
# decode the checksum
try:
crc, = struct.unpack('<H', msgbuf[-2:])
except struct.error as emsg:
raise Exception('Unable to unpack MAVLink CRC: %s' % emsg)
crcbuf = msgbuf[1:-2]
crcbuf = crcbuf + struct.pack('B',crc_extra)
crc2 = x25crc(crcbuf)
if crc != crc2.crc:
raise Exception('invalid MAVLink CRC in msgID %u 0x%04x should be 0x%04x' % (msgId, crc, crc2.crc))
try:
t = struct.unpack(fmt, msgbuf[6:-2])
except struct.error as emsg:
raise Exception('Unable to unpack MAVLink payload type=%s fmt=%s payloadLength=%u: %s' % (
type, fmt, len(msgbuf[6:-2]), emsg))
tlist = list(t)
# handle sorted fields
if True:
t = tlist[:]
if sum(len_map) == len(len_map):
# message has no arrays in it
for i in range(0, len(tlist)):
tlist[i] = t[order_map[i]]
else:
# message has some arrays
tlist = []
for i in range(0, len(order_map)):
order = order_map[i]
L = len_map[order]
tip = sum(len_map[:order])
field = t[tip]
if L == 1 or isinstance(field, str):
tlist.append(field)
else:
tlist.append(t[tip:(tip + L)])
# terminate any strings
for i in range(0, len(tlist)):
if isinstance(tlist[i], str):
tlist[i] = str(mavlink.MAVString(tlist[i]))
t = tuple(tlist)
# construct the message object
try:
m = type(*t)
except Exception as emsg:
raise Exception('Unable to instantiate MAVLink message of type %s : %s' % (type, emsg))
m._msgbuf = msgbuf
m._payload = msgbuf[6:-2]
m._crc = crc
m._header = mavlink.MAVLink_header(msgId, mlen, seq, srcSystem, srcComponent)
self._value = m
return m
def _decode(self, msgbuf):
'''
Decode a buffer as a MAVLink message
'''
try:
magic, mlen, seq, srcSystem, srcComponent, msgId = struct.unpack(
'cBBBBB', msgbuf[:6])
except struct.error as emsg:
raise Exception('Unable to unpack MAVLink header: %s' % emsg)
if ord(magic) != 254:
raise Exception("invalid MAVLink prefix '%s'" % magic)
if mlen != len(msgbuf) - 8:
raise Exception(
'invalid MAVLink message length. Got %u expected %u, msgId=%u'
% (len(msgbuf) - 8, mlen, msgId))
if not msgId in mavlink.mavlink_map:
raise Exception('unknown MAVLink message ID %u' % msgId)
# decode the payload
type = mavlink.mavlink_map[msgId]
fmt = type.format
order_map = type.orders
len_map = type.lengths
crc_extra = type.crc_extra
# decode the checksum
try:
crc, = struct.unpack('<H', msgbuf[-2:])
except struct.error as emsg:
raise Exception('Unable to unpack MAVLink CRC: %s' % emsg)
crcbuf = msgbuf[1:-2]
crcbuf = crcbuf + struct.pack('B', crc_extra)
crc2 = x25crc(crcbuf)
if crc != crc2.crc:
raise Exception(
'invalid MAVLink CRC in msgID %u 0x%04x should be 0x%04x' %
(msgId, crc, crc2.crc))
try:
t = struct.unpack(fmt, msgbuf[6:-2])
except struct.error as emsg:
raise Exception(
'Unable to unpack MAVLink payload type=%s fmt=%s payloadLength=%u: %s'
% (type, fmt, len(msgbuf[6:-2]), emsg))
tlist = list(t)
# handle sorted fields
if True:
t = tlist[:]
if sum(len_map) == len(len_map):
# message has no arrays in it
for i in range(0, len(tlist)):
tlist[i] = t[order_map[i]]
else:
# message has some arrays
tlist = []
for i in range(0, len(order_map)):
order = order_map[i]
L = len_map[order]
tip = sum(len_map[:order])
field = t[tip]
if L == 1 or isinstance(field, str):
tlist.append(field)
else:
tlist.append(t[tip:(tip + L)])
# terminate any strings
for i in range(0, len(tlist)):
if isinstance(tlist[i], str):
tlist[i] = str(mavlink.MAVString(tlist[i]))
t = tuple(tlist)
# construct the message object
try:
m = type(*t)
except Exception as emsg:
raise Exception(
'Unable to instantiate MAVLink message of type %s : %s' %
(type, emsg))
m._msgbuf = msgbuf
m._payload = msgbuf[6:-2]
m._crc = crc
m._header = mavlink.MAVLink_header(msgId, mlen, seq, srcSystem,
srcComponent)
self._value = m
return m
