|
|
|
@ -1,6 +1,61 @@
@@ -1,6 +1,61 @@
|
|
|
|
|
clear all |
|
|
|
|
clc |
|
|
|
|
|
|
|
|
|
%%%%%%%%%%%%%%%%%%%%%%% |
|
|
|
|
% SYSTEM VECTOR |
|
|
|
|
% |
|
|
|
|
% All measurements in NED frame |
|
|
|
|
% |
|
|
|
|
% uint64_t timestamp; |
|
|
|
|
% float gyro[3]; in rad/s |
|
|
|
|
% float accel[3]; in m/s^2 |
|
|
|
|
% float mag[3]; in Gauss |
|
|
|
|
% float baro; pressure in millibar |
|
|
|
|
% float baro_alt; altitude above MSL in meters |
|
|
|
|
% float baro_temp; in degrees celcius |
|
|
|
|
% float control[4]; roll, pitch, yaw [-1..1], thrust [0..1] |
|
|
|
|
% float actuators[8]; motor 1-8, in motor units (PWM: 1000-2000, |
|
|
|
|
% AR.Drone: 0-512 |
|
|
|
|
% float vbat; battery voltage in volt |
|
|
|
|
% float adc[3]; remaining auxiliary ADC ports in volt |
|
|
|
|
% float local_position |
|
|
|
|
% int32 gps_raw_position |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
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), :) = []; |
|
|
|
|
|
|
|
|
|
plot3(gps(:,1), gps(:,2), gps(:,3)); |
|
|
|
|
|
|
|
|
|
plot(sysvector(:,1), sysvector(:,2:32)); |
|
|
|
|
end |
|
|
|
|
|
|
|
|
|
if exist('actuator_outputs0.bin', 'file') |
|
|
|
|
% Read actuators file |
|
|
|
|
myFile = java.io.File('actuator_outputs0.bin') |
|
|
|
@ -9,7 +64,7 @@ if exist('actuator_outputs0.bin', 'file')
@@ -9,7 +64,7 @@ if exist('actuator_outputs0.bin', 'file')
|
|
|
|
|
fid = fopen('actuator_outputs0.bin', 'r'); |
|
|
|
|
elements = int64(fileSize./(16*4+8)) |
|
|
|
|
|
|
|
|
|
for i=1:(elements-2) |
|
|
|
|
for i=1:elements |
|
|
|
|
% timestamp |
|
|
|
|
actuators(i,1) = double(fread(fid, 1, '*uint64', 0, 'ieee-le.l64')); |
|
|
|
|
% actuators 1-16 |
|
|
|
|