forked from Archive/PX4-Autopilot
137 lines
4.3 KiB
Plaintext
137 lines
4.3 KiB
Plaintext
%% 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
|