def updatemsg(msg: bytearray) -> bytearray:
# Update packet sequence
msg[PACKET_SEQ_LOC] = updatemsg.packet_seq
if (msg[PACKET_LENGTH] == 0x1A):
msg[K1_LOC_BYTE1] = k1_command[0]
msg[K1_LOC_BYTE1 + 1] = k1_command[1]
msg[SL1_LOC_BYTE1] = sl1_command[0]
msg[SL1_LOC_BYTE1 + 1] = sl1_command[1]
msg[S1_LOC_BYTE1] = s1_command[0]
msg[S1_LOC_BYTE1 + 1] = s1_command[1]
msg[S2_LOC_BYTE1] = s2_command[0]
msg[S2_LOC_BYTE1 + 1] = s2_command[1]
# crc_init(&tempchecksum); // done by x25crc
#length from packet + 10 header bytes - CRC
# for (uint8_t j=1;j<msg[1]+8;j=j+1) {
# crc_accumulate(msg[j],&tempchecksum);
# }
crc = x25crc(msg)
# Add the CRC_EXTRA Byte which seems to be 0
#crc_accumulate(0,&tempchecksum);
crc.accumulate(b'\x00')
# Append the checksum to the message
# little endian: MSB is at end of byte array (PACKET_LENGTH+9)
# C code:
#msg[msg[1]+8]=tempchecksum & 0x00FF;
#msg[msg[1]+9]=((tempchecksum >> 8)&0x00FF);
crc_bytearray = bytearray(
crc.crc.to_bytes(2, byteorder='little', signed=False))
msg += crc_bytearray
# Increment packet sequence
if (updatemsg.packet_seq == 255):
updatemsg.packet_seq = 0
else:
updatemsg.packet_seq = updatemsg.packet_seq + 1
# Return the changed packet
return msg
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, len_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"
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, len_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"
