def decode(self, msgbuf):
'''decode a buffer as a MAVLink message'''
# decode the header
try:
magic, mlen, seq, srcSystem, srcComponent, msgId = struct.unpack('cBBBBB', msgbuf[:6])
except struct.error as emsg:
raise MAVError('Unable to unpack MAVLink header: %s' % emsg)
if ord(magic) != 254:
raise MAVError("invalid MAVLink prefix '%s'" % magic)
if mlen != len(msgbuf)-8:
raise MAVError('invalid MAVLink message length. Got %u expected %u, msgId=%u' % (len(msgbuf)-8, mlen, msgId))
if not msgId in mavlink_map:
raise MAVError('unknown MAVLink message ID %u' % msgId)
# decode the payload
(fmt, type, order_map, crc_extra) = mavlink_map[msgId]
# decode the checksum
try:
crc, = struct.unpack('<H', msgbuf[-2:])
except struct.error as emsg:
raise MAVError('Unable to unpack MAVLink CRC: %s' % emsg)
crc2 = mavutil.x25crc(msgbuf[1:-2])
if True: # using CRC extra
crc2.accumulate(chr(crc_extra))
if crc != crc2.crc:
raise MAVError('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 MAVError('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[:]
for i in range(0, len(tlist)):
tlist[i] = t[order_map[i]]
# terminate any strings
for i in range(0, len(tlist)):
if isinstance(tlist[i], str):
tlist[i] = MAVString(tlist[i])
t = tuple(tlist)
# construct the message object
try:
m = type(*t)
except Exception as emsg:
raise MAVError('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_header(msgId, mlen, seq, srcSystem, srcComponent)
return m
def message_checksum(msg):
'''calculate a 8-bit checksum of the key fields of a message, so we
can detect incompatible XML changes'''
crc = mavutil.x25crc(msg.name + ' ')
for f in msg.ordered_fields:
crc.accumulate(f.type + ' ')
crc.accumulate(f.name + ' ')
if f.array_length:
crc.accumulate(chr(f.array_length))
return (crc.crc&0xFF) ^ (crc.crc>>8)
def message_checksum(msg):
'''calculate a 8-bit checksum of the key fields of a message, so we
can detect incompatible XML changes'''
crc = mavutil.x25crc(msg.name + ' ')
for f in msg.ordered_fields:
crc.accumulate(f.type + ' ')
crc.accumulate(f.name + ' ')
if f.array_length:
crc.accumulate(chr(f.array_length))
return (crc.crc & 0xFF) ^ (crc.crc >> 8)
def pack(self, mav, crc_extra, payload):
self._payload = payload
self._header = MAVLink_header(self._header.msgId, len(payload), mav.seq,
mav.srcSystem, mav.srcComponent)
self._msgbuf = self._header.pack() + payload
crc = mavutil.x25crc(self._msgbuf[1:])
if True: # using CRC extra
crc.accumulate(chr(crc_extra))
self._crc = crc.crc
self._msgbuf += struct.pack('<H', self._crc)
return self._msgbuf
def message_checksum(msg):
'''calculate a 8-bit checksum of the key fields of a message, so we
can detect incompatible XML changes'''
crc = mavutil.x25crc(msg.name + ' ')
str = msg.name + ' '
for f in msg.ordered_fields:
crc.accumulate(f.type + ' ')
str += f.type + ' '
crc.accumulate(f.name + ' ')
str += f.name + ' '
str1 = str
if f.array_length:
crc.accumulate(chr(f.array_length))
str1 += unichr(f.array_length)
if msg.id==36:
print(str1)
print((crc.crc&0xFF) ^ (crc.crc>>8))
return (crc.crc&0xFF) ^ (crc.crc>>8)
def message_checksum(msg):
'''calculate a 8-bit checksum of the key fields of a message, so we
can detect incompatible XML changes'''
crc = mavutil.x25crc(msg.name + ' ')
str = msg.name + ' '
for f in msg.ordered_fields:
crc.accumulate(f.type + ' ')
str += f.type + ' '
crc.accumulate(f.name + ' ')
str += f.name + ' '
str1 = str
if f.array_length:
crc.accumulate(chr(f.array_length))
str1 += unichr(f.array_length)
if msg.id == 36:
print(str1)
print((crc.crc & 0xFF) ^ (crc.crc >> 8))
return (crc.crc & 0xFF) ^ (crc.crc >> 8)
def convert_file(mavlink_file, pcap_file):
# the whole file is read in a bunch - room for improvement...
data = mavlink_file.read()
i=0
done = False
skipped_char = None
junk = ''
cnt_ok = 0
cnt_junk = 0
cnt_crc = 0
while not done:
i+=1
# look for potential start of frame
next_sof = find_next_frame(data)
if next_sof > 0:
print "skipped " + str(next_sof) + " bytes"
if write_junk:
if skipped_char != None:
junk = skipped_char + data[:next_sof]
skipped_char = None
write_packet(i, junk, 0x03, len(junk))
data = data[next_sof:]
data[:6]
cnt_junk += 1
# assume, our 0xFE was the start of a packet
header = parse_header(data)
payload_len = header['plength']
pkt_length = 6 + payload_len + 2
try:
pkt_crc = ULInt16('crc').parse(data[pkt_length-2:pkt_length])
except FieldError:
# ups, no more data
done = True
continue
# peek for the next SOF
try:
cc = mavutil.x25crc(data[1:6+payload_len])
cc.accumulate(chr(MAVLINK_MESSAGE_CRCS[header['msgid']]))
x25_crc = cc.crc
if x25_crc != pkt_crc:
crc_flag = 0x1
else:
crc_flag = 0
next_magic = data[pkt_length]
if chr(MAVLINK_MAGIC) != next_magic:
# damn, retry
print "packet %d has invalid length, crc error: %d" % (i, crc_flag)
# skip one char to look for a new SOF next round, stow away skipped char
skipped_char = data[0]
data = data[1:]
continue
# we can consider it a packet now
pkt = data[:pkt_length]
write_packet(i, pkt, crc_flag, len(pkt))
print "packet %d ok, crc error: %d" % (i, crc_flag)
data = data[pkt_length:]
if crc_flag:
cnt_crc += 1
else:
cnt_ok += 1
except IndexError:
# ups, no more packets
done = True
print "converted %d valid packets, %d crc errors, %d junk fragments (total %f%% of junk)" % (cnt_ok, cnt_crc, cnt_junk, 100.*float(cnt_junk+cnt_crc)/(cnt_junk+cnt_ok+cnt_crc))
def convert_file(mavlink_file, pcap_file):
# the whole file is read in a bunch - room for improvement...
data = mavlink_file.read()
i = 0
done = False
skipped_char = None
junk = ''
cnt_ok = 0
cnt_junk = 0
cnt_crc = 0
while not done:
i += 1
# look for potential start of frame
next_sof = find_next_frame(data)
if next_sof > 0:
print "skipped " + str(next_sof) + " bytes"
if write_junk:
if skipped_char != None:
junk = skipped_char + data[:next_sof]
skipped_char = None
write_packet(i, junk, 0x03, len(junk))
data = data[next_sof:]
data[:6]
cnt_junk += 1
# assume, our 0xFE was the start of a packet
header = parse_header(data)
payload_len = header['plength']
pkt_length = 6 + payload_len + 2
try:
pkt_crc = ULInt16('crc').parse(data[pkt_length - 2:pkt_length])
except FieldError:
# ups, no more data
done = True
continue
# peek for the next SOF
try:
cc = mavutil.x25crc(data[1:6 + payload_len])
cc.accumulate(chr(MAVLINK_MESSAGE_CRCS[header['msgid']]))
x25_crc = cc.crc
if x25_crc != pkt_crc:
crc_flag = 0x1
else:
crc_flag = 0
next_magic = data[pkt_length]
if chr(MAVLINK_MAGIC) != next_magic:
# damn, retry
print "packet %d has invalid length, crc error: %d" % (
i, crc_flag)
# skip one char to look for a new SOF next round, stow away skipped char
skipped_char = data[0]
data = data[1:]
continue
# we can consider it a packet now
pkt = data[:pkt_length]
write_packet(i, pkt, crc_flag, len(pkt))
print "packet %d ok, crc error: %d" % (i, crc_flag)
data = data[pkt_length:]
if crc_flag:
cnt_crc += 1
else:
cnt_ok += 1
except IndexError:
# ups, no more packets
done = True
print "converted %d valid packets, %d crc errors, %d junk fragments (total %f%% of junk)" % (
cnt_ok, cnt_crc, cnt_junk, 100. * float(cnt_junk + cnt_crc) /
(cnt_junk + cnt_ok + cnt_crc))
def check_type_of_telemetry_file(filename):
"""Find out the type of a telemetry file. For example:-
Is is an Ascii file, a binary raw mavlink file, or a mavlink
file with timestamps ? The routine returns one of:-
MAVLINK 1.0 RAW
MAVLINK 1.0 TIMESTAMPS
MAVLINK UNKNOWN
ASCII
UNKNOWN"""
try:
fd = open(filename ,"rb")
except:
print "Error: Could not open", filename
return "Error opening file"
##### Pseudo code
# Read in 1000 bytes of a file
# Check first for presence of 0xfe Mavlink 1.0 marker and then check CRC of message.
# Note positions of 0xfe, end of valid message, start of next valid message. etc.
# If all messages back to back, then this is a raw MAVLink file.
# If all messages back to back with some same space (timestamp), then timestamp file.
# if No valid MAVLink messages, examine as ASCII. If pure ascii then return Ascii.
mybuffer = fd.read(1000)
bytes = array.array('B')
if isinstance(mybuffer, array.array):
bytes.extend(mybuffer)
else:
bytes.fromstring(mybuffer)
# Find out if this buffer has valid MAVLink packets
number_of_valid_mavlink_packets = 0
mavlink_parser_states = ['looking_for_start_char','found_start_char','valid_mav_packet' \
'non_valid_mav_packet']
state = 'looking_for_start_char'
parsing_index = 0
last_end_of_packet_index = 0
number_of_packet_gaps = 0
packet_gaps = 0
while parsing_index < (len(bytes)-(255+8)) : # At least one MAVlink packet left
if state == 'looking_for_start_char' :
if bytes[parsing_index] == 254 :
state = 'found_start_char'
else:
parsing_index += 1
elif state == "found_start_char" :
# calculate crc
crc_is_ok = False
payload_length = bytes[parsing_index + 1]
if payload_length > 255 :
# Nonesensical payload length. Forget it.
state == 'looking_for_start_char'
continue
seq = bytes[parsing_index + 2]
mycrc = mavutil.x25crc()
if (parsing_index + payload_length + 7) < len(bytes) :
mycrc.accumulate(bytes[(parsing_index + 1 ):(parsing_index + payload_length + 6)])
msgId = bytes[parsing_index + 5]
(fmt, type, order_map, crc_extra) = mavlink.mavlink_map[msgId]
mycrc.accumulate(struct.pack('B',crc_extra))
low_byte_sent_crc = bytes[parsing_index + payload_length + 6]
high_byte_sent_crc = bytes[parsing_index + payload_length + 7]
total_sent_crc = low_byte_sent_crc + ( high_byte_sent_crc * 256)
if mycrc.crc == total_sent_crc :
state = 'valid_mav_packet'
if number_of_valid_mavlink_packets != 0 : # This is not the very first packet
packet_gaps += parsing_index - (last_end_of_packet_index + 1)
number_of_packet_gaps += 1
last_end_of_packet_index = parsing_index + payload_length + 7
# Move parser onto expected start of next packet.
parsing_index = parsing_index + payload_length + 8
else :
state = 'non_valid_mav_packet'
# Look for next valid 0xfe marker for start of packet
else:
# We are out of characters in our buffer for testing for packets
break
elif state == "valid_mav_packet" :
number_of_valid_mavlink_packets += 1
# Do something - like make a note of end of position of end of packet
state = 'looking_for_start_char'
elif state == 'non_valid_mav_packet' :
print "Found a potential non valid mavlink packet"
state = 'looking_for_start_char'
parsing_index += 1
else :
print "Invalid state when parsing mavlink to check type of telemetry"
parsing_index += 1
if number_of_valid_mavlink_packets >= 1 :
if number_of_packet_gaps > 0 :
gap_average = packet_gaps / number_of_packet_gaps
else :
gap_average = 0
#print "Average number of bytes between valid packets is", gap_average
if (gap_average < 0.5 ) and ( gap_average <= 0 ) :
return "MAVLINK 1.0 RAW"
elif (gap_average > 7.5) and ( gap_average < 8.5 ) :
return "MAVLINK 1.0 TIMESTAMPS"
else :
return "MAVLINK UNKNOWN"
else :
parser_index = 0
number_of_ascii_chars = 0
while parser_index < len(bytes) :
this_byte = bytes[parser_index]
if ( this_byte >= 32 ) and ( this_byte < 128 ) : # space char through to DEL char
number_of_ascii_chars += 1
elif this_byte == 13 or this_byte == 10 : # CR and NL
number_of_ascii_chars += 1
else :
pass
parser_index += 1
if len(bytes) > 0 :
average_ascii_chars = number_of_ascii_chars / float(len(bytes))
else :
average_ascii_chars = 0
#print "This file is", average_ascii_chars * 100, "percent ascii"
if average_ascii_chars > 0.98 :
return "ASCII"
else :
return "UNKNOWN"
return "ERROR while checking file types"
import mavutil
import array
print "module CanonicalCRC where"
print "-- autogenerated using mavutil x25crc"
print "crc :: [(Int, Int, Int)]"
print "crc ="
for a in range (0,256):
for b in range (0,256):
if a == 0 and b == 0:
sep = "["
else:
sep = ","
pair = array.array('B', [a, b])
print( " %s (%d, %d, %d)"
% (sep, a, b, mavutil.x25crc(pair).crc) )
print " ]"
