px4_autopilot/EKF/matlab/EKF_replay/px4_replay_import.m

%% PX4 replay: import sensors CSV
% the following variables must be set beforehand!
if ~exist('sensors_file','var')
    error('sensors_file missing');
end
if ~exist('air_data_file','var')
    error('air_data_file missing');
end
if ~exist('magnetometer_file','var')
    error('magnetometer_file missing');
end
if ~exist('gps_file','var')
    error('gps_file missing');
end

if ~exist('attitude_file','var')
    disp('INFO no attitude_file set, all ground truth data will be skipped');
end
if ~exist('localpos_file','var')
    disp('INFO no localpos_file set, all ground truth data will be skipped');
end
if ~exist('globalpos_file','var')
    disp('INFO no globalpos_file set, all ground truth data will be skipped');
end

if ~exist('actctrl_file','var')
    disp('INFO no actctrl_file set, all actuator data will be skipped');
end
if ~exist('actout_file','var')
    disp('INFO no actout_file set, all actuator data will be skipped');
end

%% ------                 SECTION 1: IMU, Baro, Mag                 ------

%% Import IMU data from text file
opts = delimitedTextImportOptions("NumVariables", 10);
opts.DataLines = [2, Inf];
opts.Delimiter = ",";

% Specify column names and types
opts.VariableNames = ["timestamp", "gyro_rad0", "gyro_rad1", "gyro_rad2", "gyro_integral_dt", "accelerometer_timestamp_relative", "accelerometer_m_s20", "accelerometer_m_s21", "accelerometer_m_s22", "accelerometer_integral_dt"];
opts.VariableTypes = ["double", "double", "double", "double", "double", "double", "double", "double", "double", "double"];
opts.ExtraColumnsRule = "ignore";
opts.EmptyLineRule = "read";

% Import the data
tbl = readtable(sensors_file, opts);

% Convert to output type
timestamp = tbl.timestamp;
gyro_rad0 = tbl.gyro_rad0;
gyro_rad1 = tbl.gyro_rad1;
gyro_rad2 = tbl.gyro_rad2;
gyro_integral_dt = tbl.gyro_integral_dt;
accelerometer_timestamp_relative = tbl.accelerometer_timestamp_relative;
accelerometer_m_s20 = tbl.accelerometer_m_s20;
accelerometer_m_s21 = tbl.accelerometer_m_s21;
accelerometer_m_s22 = tbl.accelerometer_m_s22;
accelerometer_integral_dt = tbl.accelerometer_integral_dt;

clear opts tbl

%% Import Baro data from text file
opts = delimitedTextImportOptions("NumVariables", 5);
opts.DataLines = [2, Inf];
opts.Delimiter = ",";

% Specify column names and types
opts.VariableNames = ["timestamp_baro", "baro_alt_meter", "baro_temp_celcius", "baro_pressure_pa", "rho"];
opts.VariableTypes = ["double", "double", "double", "double", "double"];
opts.ExtraColumnsRule = "ignore";
opts.EmptyLineRule = "read";

% Import the data
tbl = readtable(air_data_file, opts);

% Convert to output type
timestamp_baro = tbl.timestamp_baro;
baro_alt_meter = tbl.baro_alt_meter;
baro_temp_celcius = tbl.baro_temp_celcius;
baro_pressure_pa = tbl.baro_pressure_pa;
rho = tbl.rho;

clear opts tbl

%% Import Mag data from text file
opts = delimitedTextImportOptions("NumVariables", 4);
opts.DataLines = [2, Inf];
opts.Delimiter = ",";

% Specify column names and types
opts.VariableNames = ["timestamp_mag", "magnetometer_ga0", "magnetometer_ga1", "magnetometer_ga2"];
opts.VariableTypes = ["double", "double", "double", "double"];
opts.ExtraColumnsRule = "ignore";
opts.EmptyLineRule = "read";

% Import the data
tbl = readtable(magnetometer_file, opts);

% Convert to output type
timestamp_mag = tbl.timestamp_mag;
magnetometer_ga0 = tbl.magnetometer_ga0;
magnetometer_ga1 = tbl.magnetometer_ga1;
magnetometer_ga2 = tbl.magnetometer_ga2;

clear opts tbl

%% Run conversion script for IMU, Baro and Mag
cd Common/;
convert_px4_sensor_combined_csv_data;
cd ../;


%% ------                       SECTION 2: GPS                       ------

%% Import data from text file
opts = delimitedTextImportOptions("NumVariables", 25);
opts.DataLines = [2, Inf];
opts.Delimiter = ",";

% Specify column names and types
opts.VariableNames = ["timestamp", "time_utc_usec", "lat", "lon", "alt", "alt_ellipsoid", "s_variance_m_s", "c_variance_rad", "eph", "epv", "hdop", "vdop", "noise_per_ms", "jamming_indicator", "vel_m_s", "vel_n_m_s", "vel_e_m_s", "vel_d_m_s", "cog_rad", "timestamp_time_relative", "heading", "heading_offset", "fix_type", "vel_ned_valid", "satellites_used"];
opts.VariableTypes = ["double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double"];
opts.ExtraColumnsRule = "ignore";
opts.EmptyLineRule = "read";

% Import the data
tbl = readtable(gps_file, opts);

% Convert to output type
timestamp = tbl.timestamp;
time_utc_usec = tbl.time_utc_usec;
lat = tbl.lat;
lon = tbl.lon;
alt = tbl.alt;
alt_ellipsoid = tbl.alt_ellipsoid;
s_variance_m_s = tbl.s_variance_m_s;
c_variance_rad = tbl.c_variance_rad;
eph = tbl.eph;
epv = tbl.epv;
hdop = tbl.hdop;
vdop = tbl.vdop;
noise_per_ms = tbl.noise_per_ms;
jamming_indicator = tbl.jamming_indicator;
vel_m_s = tbl.vel_m_s;
vel_n_m_s = tbl.vel_n_m_s;
vel_e_m_s = tbl.vel_e_m_s;
vel_d_m_s = tbl.vel_d_m_s;
cog_rad = tbl.cog_rad;
timestamp_time_relative = tbl.timestamp_time_relative;
heading = tbl.heading;
heading_offset = tbl.heading_offset;
fix_type = tbl.fix_type;
vel_ned_valid = tbl.vel_ned_valid;
satellites_used = tbl.satellites_used;

clear opts tbl


%% Run conversion script for GPS
cd Common/;
convert_px4_vehicle_gps_position_csv;
cd ../;


%% ------     SECTION 3: Ground Truth Data (STIL only, optional)     ------

if exist('attitude_file','var') && exist('localpos_file','var') && exist('globalpos_file','var')
    
%- Import Attitude data from text file
opts = delimitedTextImportOptions("NumVariables", 13);
opts.DataLines = [2, Inf];
opts.Delimiter = ",";

% Specify column names and types
opts.VariableNames = ["timestamp", "rollspeed", "pitchspeed", "yawspeed", "q0", "q1", "q2", "q3", "delta_q_reset0", "delta_q_reset1", "delta_q_reset2", "delta_q_reset3", "quat_reset_counter"];
opts.VariableTypes = ["double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double"];
opts.ExtraColumnsRule = "ignore";
opts.EmptyLineRule = "read";

% Import the data
tbl = readtable(attitude_file, opts);

% Convert to output type
timestamp_att = tbl.timestamp;
rollspeed = tbl.rollspeed;
pitchspeed = tbl.pitchspeed;
yawspeed = tbl.yawspeed;
q0 = tbl.q0;
q1 = tbl.q1;
q2 = tbl.q2;
q3 = tbl.q3;
% delta_q_reset0 = tbl.delta_q_reset0;
% delta_q_reset1 = tbl.delta_q_reset1;
% delta_q_reset2 = tbl.delta_q_reset2;
% delta_q_reset3 = tbl.delta_q_reset3;
% quat_reset_counter = tbl.quat_reset_counter;

clear opts tbl


%- Import Global Position data from text file
opts = delimitedTextImportOptions("NumVariables", 17);
opts.DataLines = [2, Inf];
opts.Delimiter = ",";

% Specify column names and types
opts.VariableNames = ["timestamp", "lat", "lon", "alt", "alt_ellipsoid", "delta_alt", "vel_n", "vel_e", "vel_d", "yaw", "eph", "epv", "terrain_alt", "lat_lon_reset_counter", "alt_reset_counter", "terrain_alt_valid", "dead_reckoning"];
opts.VariableTypes = ["double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double"];
opts.ExtraColumnsRule = "ignore";
opts.EmptyLineRule = "read";

% Import the data
tbl = readtable(globalpos_file, opts);

% Convert to output type
timestamp_gpos = tbl.timestamp;
gpos_lat = tbl.lat;
gpos_lon = tbl.lon;
gpos_alt = tbl.alt;
% gpos_alt_ellipsoid = tbl.alt_ellipsoid;
% gpos_delta_alt = tbl.delta_alt;
gpos_vel_n = tbl.vel_n;
gpos_vel_e = tbl.vel_e;
gpos_vel_d = tbl.vel_d;
% gpos_yaw = tbl.yaw;
% gpos_eph = tbl.eph;
% gpos_epv = tbl.epv;
% gpos_terrain_alt = tbl.terrain_alt;
% gpos_lat_lon_reset_counter = tbl.lat_lon_reset_counter;
% gpos_alt_reset_counter = tbl.alt_reset_counter;
% gpos_terrain_alt_valid = tbl.terrain_alt_valid;
% gpos_dead_reckoning = tbl.dead_reckoning;

% Clear temporary variables
clear opts tbl


%- Import Local Position data from text file
opts = delimitedTextImportOptions("NumVariables", 43);
opts.DataLines = [2, Inf];
opts.Delimiter = ",";

% Specify column names and types
opts.VariableNames = ["timestamp", "ref_timestamp", "ref_lat", "ref_lon", "x", "y", "z", "delta_xy0", "delta_xy1", "delta_z", "vx", "vy", "vz", "z_deriv", "delta_vxy0", "delta_vxy1", "delta_vz", "ax", "ay", "az", "yaw", "ref_alt", "dist_bottom", "dist_bottom_rate", "eph", "epv", "evh", "evv", "vxy_max", "vz_max", "hagl_min", "hagl_max", "xy_valid", "z_valid", "v_xy_valid", "v_z_valid", "xy_reset_counter", "z_reset_counter", "vxy_reset_counter", "vz_reset_counter", "xy_global", "z_global", "dist_bottom_valid"];
opts.VariableTypes = ["double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double"];
opts.ExtraColumnsRule = "ignore";
opts.EmptyLineRule = "read";

% Import the data
tbl = readtable(localpos_file, opts);

% Convert to output type
timestamp_lpos = tbl.timestamp;
% lpos_ref_timestamp = tbl.ref_timestamp;
lpos_ref_lat = tbl.ref_lat;
lpos_ref_lon = tbl.ref_lon;
lpos_x = tbl.x;
lpos_y = tbl.y;
lpos_z = tbl.z;
% lpos_delta_xy0 = tbl.delta_xy0;
% lpos_delta_xy1 = tbl.delta_xy1;
% lpos_delta_z = tbl.delta_z;
lpos_vx = tbl.vx;
lpos_vy = tbl.vy;
lpos_vz = tbl.vz;
% lpos_z_deriv = tbl.z_deriv;
% lpos_delta_vxy0 = tbl.delta_vxy0;
% lpos_delta_vxy1 = tbl.delta_vxy1;
% lpos_delta_vz = tbl.delta_vz;
% lpos_ax = tbl.ax;
% lpos_ay = tbl.ay;
% lpos_az = tbl.az;
lpos_yaw = tbl.yaw;
% ref_alt = tbl.ref_alt;
% dist_bottom = tbl.dist_bottom;
% dist_bottom_rate = tbl.dist_bottom_rate;
% eph = tbl.eph;
% epv = tbl.epv;
% evh = tbl.evh;
% evv = tbl.evv;
% vxy_max = tbl.vxy_max;
% vz_max = tbl.vz_max;
% hagl_min = tbl.hagl_min;
% hagl_max = tbl.hagl_max;
% xy_valid = tbl.xy_valid;
% z_valid = tbl.z_valid;
% v_xy_valid = tbl.v_xy_valid;
% v_z_valid = tbl.v_z_valid;
% xy_reset_counter = tbl.xy_reset_counter;
% z_reset_counter = tbl.z_reset_counter;
% vxy_reset_counter = tbl.vxy_reset_counter;
% vz_reset_counter = tbl.vz_reset_counter;
% xy_global = tbl.xy_global;
% z_global = tbl.z_global;
% dist_bottom_valid = tbl.dist_bottom_valid;

% Clear temporary variables
clear opts tbl


%- Run conversion script for GPS
cd Common/;
convert_px4_groundtruth_csv_data;
cd ../;

end


%% ------          SECTION 4: Actuator Controls (optional)          ------

if exist('actctrl_file','var') && exist('actout_file','var')

%- Import Actuator Control data from text file
opts = delimitedTextImportOptions("NumVariables", 10);
opts.DataLines = [2, Inf];
opts.Delimiter = ",";

% Specify column names and types
opts.VariableNames = ["timestamp", "timestamp_sample", "control0", "control1", "control2", "control3", "control4", "control5", "control6", "control7"];
opts.VariableTypes = ["double", "double", "double", "double", "double", "double", "double", "double", "double", "double"];
opts.ExtraColumnsRule = "ignore";
opts.EmptyLineRule = "read";

% Import the data
tbl = readtable(actctrl_file, opts);

% Convert to output type
timestamp_actctrl = tbl.timestamp;
%timestamp_sample = tbl.timestamp_sample;
control0 = tbl.control0;
control1 = tbl.control1;
control2 = tbl.control2;
control3 = tbl.control3;
% control4 = tbl.control4;
% control5 = tbl.control5;
% control6 = tbl.control6;
% control7 = tbl.control7;

% Clear temporary variables
clear opts tbl


%- Import Actuator Output data from text file
opts = delimitedTextImportOptions("NumVariables", 18);
opts.DataLines = [2, Inf];
opts.Delimiter = ",";

% Specify column names and types
opts.VariableNames = ["timestamp", "noutputs", "output0", "output1", "output2", "output3", "output4", "output5", "output6", "output7", "output8", "output9", "output10", "output11", "output12", "output13", "output14", "output15"];
opts.VariableTypes = ["double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double"];
opts.ExtraColumnsRule = "ignore";
opts.EmptyLineRule = "read";

% Import the data
tbl = readtable(actout_file, opts);

% Convert to output type
timestamp_actout = tbl.timestamp;
% noutputs = tbl.noutputs;
output0 = tbl.output0;
output1 = tbl.output1;
output2 = tbl.output2;
output3 = tbl.output3;
% output4 = tbl.output4;
% output5 = tbl.output5;
% output6 = tbl.output6;
% output7 = tbl.output7;
% output8 = tbl.output8;
% output9 = tbl.output9;
% output10 = tbl.output10;
% output11 = tbl.output11;
% output12 = tbl.output12;
% output13 = tbl.output13;
% output14 = tbl.output14;
% output15 = tbl.output15;

% Clear temporary variables
clear opts tbl


%- Run conversion script for GPS
cd Common/;
convert_px4_actuators_csv_data;
cd ../;

end