1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
|
%% 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
|
