px4-firmware/apps/sdlog/mfile.template

137 lines
4.3 KiB
Plaintext
Raw Normal View History

%% Import logfiles
if (exist ('OCTAVE_VERSION', 'builtin'))
% Octave
graphics_toolkit ("fltk")
else
% Matlab
end
% define log file and GPSs
logfileFolder = 'logfiles';
fileName = 'all';
fileEnding = 'px4log';
numberOfLogTypes = length(logTypes);
path = [fileName,'.', fileEnding];
fid = fopen(path, 'r');
% get file length
fseek(fid, 0,'eof');
fileLength = ftell(fid);
fseek(fid, 0,'bof');
% get length of one block
blockLength = 4; % check: $$$$
for i=1:numberOfLogTypes
blockLength = blockLength + logTypes{i}.type_bytes*logTypes{i}.number_of_array;
end
% determine number of entries
entries = fileLength / blockLength;
% import data
offset = 0;
for i=1:numberOfLogTypes
data.(genvarname(logTypes{i}.data)).(genvarname(logTypes{i}.variable_name)) = transpose(fread(fid,[logTypes{i}.number_of_array,entries],[num2str(logTypes{i}.number_of_array),'*',logTypes{i}.type_name,'=>',logTypes{i}.type_name],blockLength-logTypes{i}.type_bytes*logTypes{i}.number_of_array));
offset = offset + logTypes{i}.type_bytes*logTypes{i}.number_of_array;
fseek(fid, offset,'bof');
progressPercentage = i/numberOfLogTypes*100;
fprintf('%3.0f%%',progressPercentage);
end
%% Plots
figure
plot(data.sensors_raw.timestamp,data.sensors_raw.gyro_raw)
figure
plot(data.sensors_raw.timestamp,data.sensors_raw.accelerometer_raw)
%% Check for lost data
% to detect lost frames (either when logging to sd card or if no new data is
% data is available for some time)
diff_counter = diff(data.sensors_raw.gyro_raw_counter);
figure
plot(diff_counter)
% to detect how accurate the timing was
diff_timestamp = diff(data.sensors_raw.timestamp);
figure
plot(diff_timestamp)
%% Export to file for google earth
if(isfield(data.gps,'lat') && isfield(data.gps,'lon') && isfield(data.gps,'alt'))
% extract coordinates and height where they are not zero
maskWhereNotZero = ((data.gps.lon ~= 0 & data.gps.lat ~= 0 ) & data.gps.alt ~= 0);
% plot
figure
plot3(data.gps.lon(maskWhereNotZero),data.gps.lat(maskWhereNotZero),data.gps.alt(maskWhereNotZero))
% create a kml file according to https://developers.google.com/kml/documentation/kml_tut
% also see https://support.google.com/earth/bin/answer.py?hl=en&answer=148072&topic=2376756&ctx=topic
% open file and overwrite content
fileId = fopen('gps_path.kml','w+');
% define strings that should be written to file
fileStartDocumentString = ['<?xml version="1.0" encoding="UTF-8"?><kml xmlns="http://www.opengis.net/kml/2.2"><Document><name>Paths</name><description>Path</description>'];
fileStyleString = '<Style id="blueLinebluePoly"><LineStyle><color>7fff0000</color><width>4</width></LineStyle><PolyStyle><color>7fff0000</color></PolyStyle></Style>';
filePlacemarkString = '<Placemark><name>Absolute Extruded</name><description>Transparent blue wall with blue outlines</description><styleUrl>#blueLinebluePoly</styleUrl><LineString><extrude>1</extrude><tessellate>1</tessellate><altitudeMode>absolute</altitudeMode><coordinates>';
fileEndPlacemarkString = '</coordinates></LineString></Placemark>';
fileEndDocumentString = '</Document></kml>';
% start writing to file
fprintf(fileId,fileStartDocumentString);
fprintf(fileId,fileStyleString);
fprintf(fileId,filePlacemarkString);
lonTemp = double(data.gps.lon(maskWhereNotZero))/1E7;
latTemp = double(data.gps.lat(maskWhereNotZero))/1E7;
altTemp = double(data.gps.alt(maskWhereNotZero))/1E3 + 100.0; % in order to see the lines above ground
% write coordinates to file
for k=1:length(data.gps.lat(maskWhereNotZero))
if(mod(k,10)==0)
fprintf(fileId,'%.10f,%.10f,%.10f\\n',lonTemp(k),latTemp(k),altTemp(k));
end
end
% write end placemark
fprintf(fileId,fileEndPlacemarkString);
% write end of file
fprintf(fileId,fileEndDocumentString);
% close file, it should now be readable in Google Earth using File -> Open
fclose(fileId);
end
if(isfield(data.position,'lat') && isfield(data.position,'lon') && isfield(data.position,'alt'))
% extract coordinates and height where they are not zero
maskWhereNotZero = ((data.position.lon ~= 0 & data.position.lat ~= 0 ) & data.position.alt ~= 0);
% plot
figure
plot3(data.position.lon(maskWhereNotZero),data.position.lat(maskWhereNotZero),data.position.alt(maskWhereNotZero))
end