forked from Archive/PX4-Autopilot
Merge pull request #101 from julianoes/hotfix_matlab_import_script
Greatly sped up Matlab import script
This commit is contained in:
Lorenz Meier
commit
f40e4d13aa
|
|
@ -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
|
||||
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue