diff --git a/matlab/analysis/estimatorLogViewer.m b/matlab/analysis/estimatorLogViewer.m new file mode 100644 index 0000000000..7c28035582 --- /dev/null +++ b/matlab/analysis/estimatorLogViewer.m @@ -0,0 +1,131 @@ +function estimatorLogViewer(fname) +close all; +load(fname); + +stateFieldNames = [fieldnames(estimatorData.EST0);fieldnames(estimatorData.EST1)]; +stateFieldNames(strcmp('T',stateFieldNames) | strcmp('index',stateFieldNames) | ... + strcmp('nStat',stateFieldNames) | strcmp('fNaN',stateFieldNames) | ... + strcmp('fHealth',stateFieldNames) | strcmp('fTOut',stateFieldNames)) = []; + +innovFieldNamesT = [fieldnames(estimatorData.EST4);fieldnames(estimatorData.EST5)]; +innovFieldNamesT(strcmp('T',innovFieldNamesT) | strcmp('index',innovFieldNamesT)) = []; +innovFieldInds = cellfun(@(x)x(end)=='V',innovFieldNamesT); +innovFieldNames = innovFieldNamesT(~innovFieldInds); + + +% lay out static objects +f = figure('Units','normalized'); +set(f,'Position',[.25,.25,.5,.5]); + +tabs = uitabgroup(f); +statesTab = uitab(tabs,'Title','States'); +innovsTab = uitab(tabs,'Title','Innovations'); + +axesConfig = {'Units','Normalized','Position',[.05,.1,.65,.85],'NextPlot','add'}; +BGconfig = {'Units','Normalized','Position',[.75,.1,.2,.85]}; +TBconfig = {'Style','Checkbox','Units','Normalized','Position',[.75,0,.2,.1],'String','Show variance'}; + +statesAxes = axes(statesTab,axesConfig{:},'XLabel',text('string','Time (sec)')); +innovsAxes = axes(innovsTab,axesConfig{:},'XLabel',text('string','Time (sec)')); + +statesBG = uibuttongroup(statesTab,BGconfig{:}); +innovsBG = uibuttongroup(innovsTab,BGconfig{:}); + +statesVarToggle = uicontrol(statesTab,TBconfig{:}); +innovsVarToggle = uicontrol(innovsTab,TBconfig{:}); + +% lay out dynamic objects +stateButtons = zeros(numel(stateFieldNames),1); +for k = 1:numel(stateFieldNames) + stateButtons(k) = uicontrol(statesBG,'Style','Checkbox','Units','normalized',... + 'String',stateFieldNames{k},'Position',[0,1 - k/numel(stateFieldNames),1, 1/numel(stateFieldNames)],... + 'Callback',@(es,ed)toggleLine(es,ed,statesAxes,statesVarToggle,estimatorData.EST0,estimatorData.EST1,estimatorData.EST2,estimatorData.EST3)); +end +innovButtons = zeros(numel(innovFieldNames),1); +for k = 1:numel(innovFieldNames) + innovButtons(k) = uicontrol(innovsBG,'Style','Checkbox','Units','normalized',... + 'String',innovFieldNames{k},'Position',[0,1 - k/numel(innovFieldNames),1, 1/numel(innovFieldNames)],... + 'Callback',@(es,ed)toggleLine(es,ed,innovsAxes,innovsVarToggle,estimatorData.EST4,estimatorData.EST5,[],[])); +end + +set(statesVarToggle,'Callback',@(es,ed)toggleVariance(es,ed,statesAxes,estimatorData.EST2,estimatorData.EST3)); +set(innovsVarToggle,'Callback',@(es,ed)toggleVariance(es,ed,innovsAxes,estimatorData.EST4,estimatorData.EST5)); +end + +function toggleLine(es,~,axes,varToggle,struct1,struct2,vstruct1,vstruct2) +if es.Value == 0 + delete(findobj(axes,'UserData',es.String)); +else + if any(strcmp(es.String,fieldnames(struct1))) + dataSrc = struct1; + else + dataSrc = struct2; + end + p = plot(dataSrc.T,dataSrc.(es.String)); + set(p,'Parent',axes,'UserData',es.String); +end +if varToggle.Value == 1 + if ~isempty(vstruct1) && ~isempty(vstruct2) + toggleVariance(varToggle,[],axes,vstruct1,vstruct2); + else + toggleVariance(varToggle,[],axes,struct1,struct2); + end +end +updateLegend(axes); +end + +function toggleVariance(es,~,axes,struct1,struct2) +if es.Value == 0 + delete(findobj(axes,'Type','Patch')); +else + lines = findobj(axes,'Type','Line'); + if isempty(lines) + return; + end + stateNames = {lines.UserData}; + if any(strncmp('s',stateNames,1)) + varNames = strrep(stateNames,'s','P'); + else + varNames = strrep(stateNames,'I','IV'); + end + for k = 1:numel(varNames) + if any(strcmp(varNames{k},fieldnames(struct1))) + dataSrc = struct1; + else + dataSrc = struct2; + end + centerLineTimeFull = get(lines(k),'XData'); + centerLineDataFull = get(lines(k),'YData'); + + startTime = max(centerLineTimeFull(1),dataSrc.T(1)); + endTime = min(centerLineTimeFull(end),dataSrc.T(end)); + plotTimeFull = dataSrc.T(dataSrc.T >= startTime & dataSrc.T <= endTime); + plotDataFull = dataSrc.(varNames{k})(dataSrc.T >= startTime & dataSrc.T <= endTime); + + centerLineTime = centerLineTimeFull(centerLineTimeFull >= startTime & centerLineTimeFull <= endTime); + centerLineData = centerLineDataFull(centerLineTimeFull >= startTime & centerLineTimeFull <= endTime); + + plotDataT = sqrt(interp1(plotTimeFull,plotDataFull,centerLineTime)); + + plotTime = downsample(centerLineTime,round(numel(plotDataT)/350),0); + if strcmp('IV',varNames{k}(end-1:end)) + plotDataL = downsample(-plotDataT,round(numel(plotDataT)/350),0); + plotDataU = downsample(plotDataT,round(numel(plotDataT)/350),0); + else + plotDataL = downsample(centerLineData-plotDataT,round(numel(plotDataT)/350),0); + plotDataU = downsample(centerLineData+plotDataT,round(numel(plotDataT)/350),0); + end + p = patch(axes,[plotTime,fliplr(plotTime)],[plotDataL,fliplr(plotDataU)],lines(k).Color); + set(p,'EdgeColor','none','FaceAlpha',.3,'UserData',stateNames{k}); + end +end +end + +function updateLegend(axes) +lines = findobj(axes,'Type','Line'); +if isempty(lines) + legend(axes,'off'); +else + legend(axes,lines,{lines.UserData}); +end +end \ No newline at end of file diff --git a/matlab/analysis/importPX4log.m b/matlab/analysis/importPX4log.m new file mode 100644 index 0000000000..28ce827e10 --- /dev/null +++ b/matlab/analysis/importPX4log.m @@ -0,0 +1,188 @@ +function allData = importPX4log(fname,keep_msgs) + +% import a .px4log file +% INPUTS +% fname: path to a valid .px4log file +% keep_msgs: cell array of message names to keep +% OUTPUT +% allData: a Matlab struct with a field names for each message name in the log +% file. The content of each field is itself a struct with field names for +% each label in the corresponding message. The content of each of *these* +% fields will be an array of message data that is nonempty if the message +% name appears in keep_msgs + +% import method essentially translated from sdlog2_dump.py +% George Hines, 3D Robotics, Berkeley, CA +% 7 April 2016 + +BLOCK_SIZE = 8192; +MSG_HEADER_LEN = 3; +% MSG_HEAD1 = uint8(hex2dec('A3')); +% MSG_HEAD2 = uint8(hex2dec('95')); +MSG_FORMAT_PACKET_LEN = 89; +MSG_TYPE_FORMAT = uint8(hex2dec('80')); +% MSG_FORMAT_STRUCT = {'uint8','uint8','char4','char16','char64'}; +FORMAT_TO_STRUCT = { + 'b', {'int8', 1}; + 'B', {'uint8', 1}; + 'h', {'int16', 1}; + 'H', {'uint16', 1}; + 'i', {'int32', 1}; + 'I', {'uint32', 1}; + 'f', {'single', 1}; + 'n', {'char4', 1}; + 'N', {'char16', 1}; + 'Z', {'char64', 1}; + 'c', {'int16', 0.01}; + 'C', {'uint16', 0.01}; + 'e', {'int32', 0.01}; + 'E', {'uint32', 0.01}; + 'L', {'int32', 0.0000001}; + 'M', {'int8', 1}; + 'q', {'int64', 1}; + 'Q', {'uint64', 1}}; + +fid = fopen(fname,'r','b'); +fInfo = dir(fname); +totalBytes = fInfo.bytes; +bytes_read = 0; +msg_descrs = {}; +buffer = []; +ptr = 1; +allData = []; +nextPrint = 0; +disp('Reading file'); +while 1 + percentDone = bytes_read / totalBytes * 100; + if percentDone >= nextPrint + fprintf('%.0f%%\n',percentDone); + nextPrint = nextPrint + 5; + end + + chunk = fread(fid,BLOCK_SIZE,'uint8'); + if numel(chunk) == 0; + break + end + buffer = [buffer(ptr:end), 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 + [ptr, msg_descr] = LOCAL_parse_message_descriptors(buffer, ptr, MSG_TYPE_FORMAT, MSG_FORMAT_PACKET_LEN, FORMAT_TO_STRUCT); + msg_descrs(msg_descr{1},:) = msg_descr; + cells = repmat({inf(1,500000)},1,numel(msg_descr{5})); + cells(msg_descr{4}=='n' | msg_descr{4} == 'N' | msg_descr{4} == 'Z') = {[]}; + seed = [{'index'},{'T'},msg_descr{5};[{1},{inf(1,500000)},cells]]; + allData.(msg_descr{3}) = struct(seed{:}); + else + msg_descr = msg_descrs(msg_type,:); + msg_length = msg_descr{2}; + if numel(buffer) - ptr <= msg_length + break; + end + % return new pointer, and all message info + if strcmp(msg_descr{3},'TIME') || any(strcmp(msg_descr{3}, keep_msgs)) || isempty(keep_msgs) + [ptr,msg_data] = LOCAL_parse_message(buffer, ptr, MSG_HEADER_LEN, msg_descr); + ind = allData.(msg_descr{3}).index; + for k = 1:numel(msg_data) + if isnumeric(msg_data{k}) + allData.(msg_descr{3}).(msg_descr{5}{k})(ind) = msg_data{k}; + allData.(msg_descr{3}).T(ind) = double(allData.TIME.StartTime(max(1,allData.TIME.index-1)))*1e-6; + noInc = false; + else + allData.(msg_descr{3}).(msg_descr{5}{k}) = [allData.(msg_descr{3}).(msg_descr{5}{k}), msg_data(k)]; + noInc = true; + end + end + if ~noInc + allData.(msg_descr{3}).index = ind + 1; + end + else + ptr = ptr + msg_descr{2}; + end + end + end + bytes_read = bytes_read + ptr; +end + +disp('Releasing excess preallocated memory'); +% clean out inf values +fields1 = fieldnames(allData); +for k = 1:numel(fields1) + fields2 = fieldnames(allData.(fields1{k})); + for m = 1:numel(fields2) + if isnumeric(allData.(fields1{k}).(fields2{m})) + allData.(fields1{k}).(fields2{m})(isinf(allData.(fields1{k}).(fields2{m}))) = []; + end + end +end +disp('Done'); +end + +function [ptr, msg_descr] = LOCAL_parse_message_descriptors(buffer, ptr, MSG_TYPE_FORMAT, MSG_FORMAT_PACKET_LEN, FORMAT_TO_STRUCT) +thisBlock = buffer((ptr+3):(ptr+MSG_FORMAT_PACKET_LEN+1)); +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 = cell(1,numel(msg_format)); + msg_mults = zeros(1,numel(msg_format)); + + for k = 1:numel(msg_format) + info = FORMAT_TO_STRUCT{strcmp(msg_format(k),FORMAT_TO_STRUCT(:,1)),2}; + msg_struct{k} = info{1}; + msg_mults(k) = info{2}; + end + msg_descr = {msg_type, msg_length, msg_name, msg_format, msg_labels, msg_struct, msg_mults}; +end +ptr = ptr + MSG_FORMAT_PACKET_LEN; +end + +function [ptr, data] = LOCAL_parse_message(buffer, ptr, MSG_HEADER_LEN, msg_descr) +[~, msg_length, ~, ~, ~, msg_struct, msg_mults] = msg_descr{:}; + +% unpack message per the format specifiers in msg_struct +data = cell(1,numel(msg_struct)); +thisBytes = buffer((ptr+MSG_HEADER_LEN):(ptr+msg_length+1)); +thisPtr = 1; +for k = 1:numel(msg_struct) + % parse the data chunk per msg_struct{k} + [dataLen,data{k}] = LOCAL_unpack(thisPtr, thisBytes, msg_struct{k}, msg_mults(k)); + thisPtr = thisPtr + dataLen; +end +ptr = ptr + msg_length; +end + +function [dataLen, data] = LOCAL_unpack(thisPtr, byte_array, format_type, mult) +if strncmp('char',format_type,4) + dataLen = str2double(format_type(5:end)); + data = LOCAL_parse_string(byte_array(thisPtr:(thisPtr+dataLen))); +else + if strncmp('int',format_type,3) + dataLen = str2double(format_type(4:end))/8; + elseif strncmp('uint',format_type,4) + dataLen = str2double(format_type(5:end))/8; + elseif strcmp('single',format_type) + dataLen = 4; + end + data = typecast(uint8(byte_array(thisPtr:(thisPtr+dataLen-1))),format_type)*mult; +end +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 \ No newline at end of file diff --git a/matlab/analysis/usageSamples.m b/matlab/analysis/usageSamples.m new file mode 100644 index 0000000000..e80df9ca7c --- /dev/null +++ b/matlab/analysis/usageSamples.m @@ -0,0 +1,10 @@ +fname = 'indoor_flight_test.px4log'; + +%% entire log +wholeLog = importPX4log(fname,{}); + +%% attitude message +attitudeData = importPX4log(fname,{'ATT'}); + +%% estimator messages +estimatorData = importPX4log(fname,{'EST0','EST1','EST2','EST3','EST4','EST5','EST6'});