dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

initial commit of matlab px4 log importer

This commit is contained in:
George Hines 2016-04-06 17:12:40 -07:00
parent f86ef34782
commit 8c8653a1a2
1 changed files with 91 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

91
matlab/analysis/importPX4log.m Normal file
View File

@ -0,0 +1,91 @@
function importPX4log(fname)
fid = fopen(fname,'r','b');
bytes_read = 0;
BLOCK_SIZE = 8192;
MSG_HEADER_LEN = 3;
MSG_HEAD1 = uint8(hex2dec('A3'));
MSG_HEAD2 = uint8(hex2dec('95'));
MSG_FORMAT_PACKET_LEN = 89;
MSG_FORMAT_STRUCT = 'BB4s16s64s';
MSG_TYPE_FORMAT = uint8(hex2dec('80'));
% FORMAT_TO_STRUCT = {
% "b": ("b", None),
% "B": ("B", None),
% "h": ("h", None),
% "H": ("H", None),
% "i": ("i", None),
% "I": ("I", None),
% "f": ("f", None),
% "n": ("4s", None),
% "N": ("16s", None),
% "Z": ("64s", None),
% "c": ("h", 0.01),
% "C": ("H", 0.01),
% "e": ("i", 0.01),
% "E": ("I", 0.01),
% "L": ("i", 0.0000001),
% "M": ("b", None),
% "q": ("q", None),
% "Q": ("Q", None),
% }
while 1
chunk = fread(fid,BLOCK_SIZE,'uint8');
if numel(chunk) == 0;
break
end
buffer = chunk;
ptr = 1;
while numel(buffer) - ptr >= MSG_HEADER_LEN
head1 = buffer(ptr);
head2 = buffer(ptr+1);
msg_type = buffer(ptr+2);
if msg_type == MSG_TYPE_FORMAT
if numel(buffer) - ptr < MSG_FORMAT_PACKET_LEN
break;
end
% return new pointer, and all message descriptor info
LOCAL_parse_message_descriptors(buffer, ptr, MSG_TYPE_FORMAT, MSG_FORMAT_STRUCT, MSG_FORMAT_PACKET_LEN);
else
msg_descr = msg_descrs{msg_type,:};
msg_length = msg_descr{1};
if numel(buffer) - ptr < msg_length
break;
end
% return new pointer, and all message info
LOCAL_parse_message(buffer, MSG_HEADER_LEN, msg_descr);
end
end
bytes_read = bytes_read + ptr;
end
end
function [ptr, msg_descrs] = LOCAL_parse_message_descriptors(buffer, ptr, MSG_TYPE_FORMAT, MSG_FORMAT_STRUCT, MSG_FORMAT_PACKET_LEN)
thisBlock = buffer((ptr+3):(ptr+MSG_FORMAT_PACKET_LEN));
msg_type = thisBlock(1);
if msg_type ~= MSG_TYPE_FORMAT
msg_length = thisBlock(2);
msg_name = LOCAL_parse_string(thisBlock(3:6));
msg_format = LOCAL_parse_string(thisBlock(7:22));
msg_labels = strsplit(LOCAL_parse_string(thisBlock(23:end)),',');
msg_struct = [];
msg_mults = [];
end
ptr = ptr + MSG_FORMAT_PACKET_LEN;
end
function LOCAL_parse_message(buffer, ptr, MSG_HEADER_LEN, msg_descr)
end
function str = LOCAL_parse_string(byteArray)
firstZero = find(byteArray==0);
if ~isempty(firstZero)
str = char(byteArray(1:firstZero-1))';
else
str = char(byteArray)';
end
end