Greatly sped up Matlab import script, a 17min flight now takes 2secs to import instead of more than the actual flight time

1 files changed, 61 insertions, 205 deletions

diff --git a/ROMFS/logging/logconv.m b/ROMFS/logging/logconv.m
index 48399f1eb..bf70d6d0f 100644
--- a/ROMFS/logging/logconv.m
+++ b/ROMFS/logging/logconv.m
@@ -1,6 +1,14 @@
+% This Matlab Script can be used to import the binary logged values of the
+% PX4FMU into data that can be plotted and analyzed.
+
+% Clear everything
+clc
 clear all
 close all
 
+% Set the path to your sysvector.bin file here
+filePath = 'sysvector.bin';
+
 %%%%%%%%%%%%%%%%%%%%%%%
 % SYSTEM VECTOR
 %
@@ -21,224 +29,72 @@ close all
 % int32_t gps_raw_position[3]; //latitude [degrees] north, longitude [degrees] east, altitude above MSL [millimeter]
 % float attitude[3]; //pitch, roll, yaw [rad]
 % float rotMatrix[9]; //unitvectors
+% float actuator_control[4]; //unitvector
+% float optical_flow[4]; //roll, pitch, yaw [-1..1], thrust [0..1]
+
+% Definition of the logged values
+logFormat{1} = struct('name', 'timestamp',         'bytes', 8, 'array', 1, 'precision', 'uint64', 'machineformat', 'ieee-le.l64'); 
+logFormat{2} = struct('name', 'gyro',              'bytes', 4, 'array', 3, 'precision', 'float',   'machineformat', 'ieee-le'); 
+logFormat{3} = struct('name', 'accel',             'bytes', 4, 'array', 3, 'precision', 'float',   'machineformat', 'ieee-le'); 
+logFormat{4} = struct('name', 'mag',               'bytes', 4, 'array', 3, 'precision', 'float',   'machineformat', 'ieee-le'); 
+logFormat{5} = struct('name', 'baro',              'bytes', 4, 'array', 1, 'precision', 'float',   'machineformat', 'ieee-le'); 
+logFormat{6} = struct('name', 'baro_alt',          'bytes', 4, 'array', 1, 'precision', 'float',   'machineformat', 'ieee-le'); 
+logFormat{7} = struct('name', 'baro_temp',         'bytes', 4, 'array', 1, 'precision', 'float',   'machineformat', 'ieee-le'); 
+logFormat{8} = struct('name', 'control',           'bytes', 4, 'array', 4, 'precision', 'float',   'machineformat', 'ieee-le');
+logFormat{9} = struct('name', 'actuators',         'bytes', 4, 'array', 8, 'precision', 'float',   'machineformat', 'ieee-le'); 
+logFormat{10} = struct('name', 'vbat',             'bytes', 4, 'array', 1, 'precision', 'float',   'machineformat', 'ieee-le'); 
+logFormat{11} = struct('name', 'adc',              'bytes', 4, 'array', 3, 'precision', 'float',   'machineformat', 'ieee-le'); 
+logFormat{12} = struct('name', 'local_position',   'bytes', 4, 'array', 3, 'precision', 'float',   'machineformat', 'ieee-le'); 
+logFormat{13} = struct('name', 'gps_raw_position', 'bytes', 4, 'array', 3, 'precision', 'uint32',  'machineformat', 'ieee-le'); 
+logFormat{14} = struct('name', 'attitude',         'bytes', 4, 'array', 3, 'precision', 'float',   'machineformat', 'ieee-le');
+logFormat{15} = struct('name', 'rot_matrix',       'bytes', 4, 'array', 9, 'precision', 'float',   'machineformat', 'ieee-le');
+logFormat{16} = struct('name', 'vicon_position',   'bytes', 4, 'array', 6, 'precision', 'float',   'machineformat', 'ieee-le');
+logFormat{17} = struct('name', 'actuator_control', 'bytes', 4, 'array', 4, 'precision', 'float',   'machineformat', 'ieee-le');
+logFormat{18} = struct('name', 'optical_flow',     'bytes', 4, 'array', 6, 'precision', 'float',   'machineformat', 'ieee-le');
+
+
+% First get length of one line
+columns = length(logFormat);
+lineLength = 0;
+
+for i=1:columns
+    lineLength = lineLength + logFormat{i}.bytes * logFormat{i}.array;
+end
 
-%myPath = '..\LOG30102012\session0002\';    %set relative path here
-myPath = '.\';                             
-myFile = 'sysvector.bin';
-filePath = strcat(myPath,myFile);
 
 if exist(filePath, 'file')
+    
     fileInfo = dir(filePath);
     fileSize = fileInfo.bytes;
     
-    fid = fopen(filePath, 'r');
-    elements = int64(fileSize./(8+(3+3+3+1+1+1+4+8+1+3+3+3+3+9+6+4+6)*4))
+    elements = int64(fileSize./(lineLength))
     
-    for i=1:elements       
-        % timestamp
-        sensors(i,1) = double(fread(fid, 1, '*uint64', 0, 'ieee-le.l64'));
-        
-        % gyro (3 channels)
-        sensors(i,2:4) = fread(fid, 3, 'float', 0, 'ieee-le');
-       
-        % accelerometer (3 channels)
-        sensors(i,5:7) = fread(fid, 3, 'float', 0, 'ieee-le');
-      
-        % mag (3 channels)
-        sensors(i,8:10) = fread(fid, 3, 'float', 0, 'ieee-le');
-       
-        % baro pressure
-        sensors(i,11) = fread(fid, 1, 'float', 0, 'ieee-le');
-        
-        % baro alt
-        sensors(i,12) = fread(fid, 1, 'float', 0, 'ieee-le');
-         
-        % baro temp
-        sensors(i,13) = fread(fid, 1, 'float', 0, 'ieee-le');
-        
-        % actuator control (4 channels) 
-        sensors(i,14:17) = fread(fid, 4, 'float', 0, 'ieee-le');
-        
-        % actuator outputs (8 channels)
-        sensors(i,18:25) = fread(fid, 8, 'float', 0, 'ieee-le');
-                
-        % vbat
-        sensors(i,26) = fread(fid, 1, 'float', 0, 'ieee-le');
-        
-        % adc voltage (3 channels)
-        sensors(i,27:29) = fread(fid, 3, 'float', 0, 'ieee-le');
-        
-        % local position (3 channels)
-        sensors(i,30:32) = fread(fid, 3, 'float', 0, 'ieee-le');
-        
-        % gps_raw_position (3 channels)
-        sensors(i,33:35) = fread(fid, 3, 'uint32', 0, 'ieee-le');
-        
-        % attitude (3 channels)
-        sensors(i,36:38) = fread(fid, 3, 'float', 0, 'ieee-le');
-        
-        % RotMatrix (9 channels)
-        sensors(i,39:47) = fread(fid, 9, 'float', 0, 'ieee-le');
-
-        % vicon position (6 channels)
-        sensors(i,48:53) = fread(fid, 6, 'float', 0, 'ieee-le');
-
-        % actuator control effective (4 channels) 
-        sensors(i,54:57) = fread(fid, 4, 'float', 0, 'ieee-le');
-
-        % optical flow (6 values) 
-        sensors(i,58:63) = fread(fid, 6, 'float', 0, 'ieee-le');
+    fid = fopen(filePath, 'r');
+    offset = 0;
+    for i=1:columns
+        % using fread with a skip speeds up the import drastically, do not
+        % import the values one after the other
+        sysvector.(genvarname(logFormat{i}.name)) = transpose(fread(...
+            fid, ...
+            [logFormat{i}.array, elements], [num2str(logFormat{i}.array),'*',logFormat{i}.precision,'=>',logFormat{i}.precision], ...
+            lineLength - logFormat{i}.bytes*logFormat{i}.array, ...
+            logFormat{i}.machineformat) ...
+        );
+        offset = offset + logFormat{i}.bytes*logFormat{i}.array;
+        fseek(fid, offset,'bof');
     end
-    time_us = sensors(elements,1) - sensors(1,1);
+    
+    % shot the flight time
+    time_us = sysvector.timestamp(end) - sysvector.timestamp(1);
     time_s = time_us*1e-6
     time_m = time_s/60
+    
+    % close the logfile
+    fclose(fid);
+    
     disp(['end log2matlab conversion' char(10)]);
 else
     disp(['file: ' filePath ' does not exist' char(10)]);
 end
 
 
-%% old version of reading in different files from sdlog.c
-% if exist('sysvector.bin', 'file')
-%     % Read actuators file
-%     myFile = java.io.File('sysvector.bin')
-%     fileSize = length(myFile)
-% 
-%     fid = fopen('sysvector.bin', 'r');
-%     elements = int64(fileSize./(8+(3+3+3+1+1+1+4+8+4+3+3)*4));
-% 
-%     for i=1:elements
-%         % timestamp
-%         sysvector(i,1) = double(fread(fid, 1, '*uint64', 0, 'ieee-le.l64'));
-%         % actuators 1-16
-%         % quadrotor: motor 1-4 on the first four positions
-%         sysvector(i, 2:32) = fread(fid, 28+3, 'float', 'ieee-le');
-%         sysvector(i,33:35) = fread(fid, 3, 'int32', 'ieee-le');
-%     end
-%     
-%     sysvector_interval_seconds = (sysvector(end,1) - sysvector(1:1)) / 1000000
-%     sysvector_minutes = sysvector_interval_seconds / 60
-%     
-%     % Normalize time
-%     sysvector(:,1) = (sysvector(:,1) - sysvector(1,1)) / 1000000;
-%     
-%     % Create some basic plots
-%     
-%     % Remove zero rows from GPS
-%     gps = sysvector(:,33:35);
-%     gps(~any(gps,2), :) = [];
-%     
-%     all_data = figure('Name', 'GPS RAW');
-%     gps_position = plot3(gps(:,1), gps(:,2), gps(:,3));
-%     
-%     
-%     all_data = figure('Name', 'Complete Log Data (exc. GPS)');
-%     plot(sysvector(:,1), sysvector(:,2:32));
-%     
-%     actuator_inputs = figure('Name', 'Attitude controller outputs');
-%     plot(sysvector(:,1), sysvector(:,14:17));
-%     legend('roll motor setpoint', 'pitch motor setpoint', 'yaw motor setpoint', 'throttle motor setpoint');
-%     
-%     actuator_outputs = figure('Name', 'Actuator outputs');
-%     plot(sysvector(:,1), sysvector(:,18:25));
-%     legend('actuator 0', 'actuator 1', 'actuator 2', 'actuator 3', 'actuator 4', 'actuator 5', 'actuator 6', 'actuator 7');
-%     
-% end
-% 
-% if exist('actuator_outputs0.bin', 'file')
-%     % Read actuators file
-%     myFile = java.io.File('actuator_outputs0.bin')
-%     fileSize = length(myFile)
-% 
-%     fid = fopen('actuator_outputs0.bin', 'r');
-%     elements = int64(fileSize./(16*4+8))
-% 
-%     for i=1:elements
-%         % timestamp
-%         actuators(i,1) = double(fread(fid, 1, '*uint64', 0, 'ieee-le.l64'));
-%         % actuators 1-16
-%         % quadrotor: motor 1-4 on the first four positions
-%         actuators(i, 2:17) = fread(fid, 16, 'float', 'ieee-le');
-%     end
-% end
-% 
-% if exist('actuator_controls0.bin', 'file')
-%     % Read actuators file
-%     myFile = java.io.File('actuator_controls0.bin')
-%     fileSize = length(myFile)
-% 
-%     fid = fopen('actuator_controls0.bin', 'r');
-%     elements = int64(fileSize./(8*4+8))
-% 
-%     for i=1:elements
-%         % timestamp
-%         actuator_controls(i,1) = fread(fid, 1, 'uint64', 0, 'ieee-le.l64');
-%         % actuators 1-16
-%         % quadrotor: motor 1-4 on the first four positions
-%         actuator_controls(i, 2:9) = fread(fid, 8, 'float', 'ieee-le');
-%     end
-% end
-% 
-% 
-% if exist('sensor_combined.bin', 'file')
-%     % Read sensor combined file
-%     % Type definition: Firmware/apps/uORB/topics/sensor_combined.h
-%     % Struct: sensor_combined_s
-%     fileInfo = dir('sensor_combined.bin');
-%     fileSize = fileInfo.bytes;
-% 
-%     fid = fopen('sensor_combined.bin', 'r');
-% 
-%     for i=1:elements
-%         % timestamp
-%         sensors(i,1) = double(fread(fid, 1, '*uint64', 0, 'ieee-le.l64'));
-%         % gyro raw
-%         sensors(i,2:4) = fread(fid, 3, 'int16', 0, 'ieee-le');
-%         % gyro counter
-%         sensors(i,5) = fread(fid, 1, 'uint16', 0, 'ieee-le');
-%         % gyro in rad/s
-%         sensors(i,6:8) = fread(fid, 3, 'float', 0, 'ieee-le');
-% 
-%         % accelerometer raw
-%         sensors(i,9:11) = fread(fid, 3, 'int16', 0, 'ieee-le');
-%         % padding bytes
-%         fread(fid, 1, 'int16', 0, 'ieee-le');
-%         % accelerometer counter
-%         sensors(i,12) = fread(fid, 1, 'uint32', 0, 'ieee-le');
-%         % accel in m/s2
-%         sensors(i,13:15) = fread(fid, 3, 'float', 0, 'ieee-le');
-%         % accel mode
-%         sensors(i,16) = fread(fid, 1, 'int32', 0, 'ieee-le');
-%         % accel range
-%         sensors(i,17) = fread(fid, 1, 'float', 0, 'ieee-le');
-% 
-%         % mag raw
-%         sensors(i,18:20) = fread(fid, 3, 'int16', 0, 'ieee-le');
-%         % padding bytes
-%         fread(fid, 1, 'int16', 0, 'ieee-le');
-%         % mag in Gauss
-%         sensors(i,21:23) = fread(fid, 3, 'float', 0, 'ieee-le');
-%         % mag mode
-%         sensors(i,24) = fread(fid, 1, 'int32', 0, 'ieee-le');
-%         % mag range
-%         sensors(i,25) = fread(fid, 1, 'float', 0, 'ieee-le');
-%         % mag cuttoff freq
-%         sensors(i,26) = fread(fid, 1, 'float', 0, 'ieee-le');
-%         % mag counter
-%         sensors(i,27) = fread(fid, 1, 'int32', 0, 'ieee-le');
-% 
-%         % baro pressure millibar
-%         % baro alt meter
-%         % baro temp celcius
-%         % battery voltage
-%         % adc voltage (3 channels)
-%         sensors(i,28:34) = fread(fid, 7, 'float', 0, 'ieee-le');
-%         % baro counter and battery counter
-%         sensors(i,35:36) = fread(fid, 2, 'uint32', 0, 'ieee-le');
-%         % battery voltage valid flag
-%         sensors(i,37) = fread(fid, 1, 'uint32', 0, 'ieee-le');
-% 
-%     end
-% end
-
-