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)))