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

Merge pull request #89 from georgehines/matlab-log-import 

Matlab log import
This commit is contained in:
Paul Riseborough 2016-04-07 15:44:04 -07:00
parent f86ef34782 684b41494e
commit a919088b6a
3 changed files with 329 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

131
matlab/analysis/estimatorLogViewer.m Normal file
View File

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

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

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

10
matlab/analysis/usageSamples.m Normal file
View File

@ -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'});