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 " ]"