Ejemplo n.º 1
0
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
Ejemplo n.º 2
0
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))
Ejemplo n.º 3
0
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"
Ejemplo n.º 4
0
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)))
Ejemplo n.º 5
0
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"