Skip to content

Commit

Permalink
Greatly sped up Matlab import script, a 17min flight now takes 2secs …
Browse files Browse the repository at this point in the history 
…to import instead of more than the actual flight time
  • Loading branch information
Julian Oes committed Dec 15, 2012
1 parent 1540352 commit 4bc3f80
Showing 1 changed file with 61 additions and 205 deletions.
266 changes: 61 additions & 205 deletions ROMFS/logging/logconv.m
View file
Original file line number Diff line number Diff line change
@@ -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
%
Expand All @@ -21,224 +29,72 @@
% 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


0 comments on commit 4bc3f80

Please sign in to comment.