px4-firmware/EKF/matlab/EKF_replay/Common/convert_apm_data.m

%% convert baro data
clear baro_data;
last_time = 0;
output_index = 1;
for source_index = 1:length(BARO)
    if (BARO(source_index,2) ~= last_time)
        baro_data.time_us(output_index,1) = BARO(source_index,2);
        baro_data.height(output_index) = BARO(source_index,3);
        last_time = BARO(source_index,2);
        output_index = output_index + 1;
    end
end
save baro_data.mat baro_data;

%% extract IMU delta angles and velocity data
clear imu_data;
imu_data.time_us = IMT(:,2);
imu_data.gyro_dt = IMT(:,5);
imu_data.del_ang = IMT(:,6:8);
imu_data.accel_dt = IMT(:,4);
imu_data.del_vel = IMT(:,9:11);
save imu_data.mat imu_data;

%% convert magnetomer data
clear mag_data;
last_time = 0;
output_index = 1;
for source_index = 1:length(MAG)
    mag_timestamp = MAG(source_index,2);
    if (mag_timestamp ~= last_time)
        mag_data.time_us(output_index,1) = mag_timestamp;
        mag_data.field_ga(output_index,:) = 0.001*[MAG(source_index,3),MAG(source_index,4),MAG(source_index,5)];
        last_time = mag_timestamp;
        output_index = output_index + 1;
    end
end
save mag_data.mat mag_data;

%% save GPS daa
clear gps_data;

maxindex = min(length(GPS),length(GPA));

gps_data.time_us = GPS(1:maxindex,2);
gps_data.pos_error = GPA(1:maxindex,4);
gps_data.spd_error = GPA(1:maxindex,6);
gps_data.hgt_error = GPA(1:maxindex,5);

% set reference point used to set NED origin when GPS accuracy is sufficient
gps_data.start_index = max(find(gps_data.pos_error < 5.0, 1 ),find(gps_data.spd_error < 1.0, 1 ));
gps_data.refLLH = [GPS(gps_data.start_index,8);GPS(gps_data.start_index,9);GPS(gps_data.start_index,10)];

% convert GPS data to NED
deg2rad = pi/180;
for index = 1:1:maxindex
    if (GPS(index,3) >= 3)
        gps_data.pos_ned(index,:) = LLH2NED([GPS(index,8);GPS(index,9);GPS(index,10)],gps_data.refLLH);
        gps_data.vel_ned(index,:) = [GPS(index,11).*cos(deg2rad*GPS(index,12)),GPS(index,11).*sin(deg2rad*GPS(index,12)),GPS(index,13)];
    else
        gps_data.pos_ned(index,:) = [0,0,0];
        gps_data.vel_ned(index,:) = [0,0,0];
    end
end

save gps_data.mat gps_data;

%% save range finder data
clear rng_data;
if (exist('RFND','var'))
    rng_data.time_us = RFND(:,2);
    rng_data.dist = RFND(:,3);
    save rng_data.mat rng_data;
end

%% save optical flow data
clear flow_data;
if (exist('OF','var'))
    flow_data.time_us = OF(:,2);
    flow_data.qual = OF(:,3)/255; % scale quality from 0 to 1
    flow_data.flowX = OF(:,4); % optical flow rate about the X body axis (rad/sec)
    flow_data.flowY = OF(:,5); % optical flow rate about the Y body axis (rad/sec)
    flow_data.bodyX = OF(:,6); % angular rate about the X body axis (rad/sec)
    flow_data.bodyY = OF(:,7); % angular rate about the Y body axis (rad/sec)
    save flow_data.mat flow_data;
end

%% save visual odometry data
clear viso_data;
if (exist('VISO','var'))
    viso_data.time_us = VISO(:,2);
    viso_data.dt = VISO(:,3); % time period the measurement was sampled across (sec)
    viso_data.dAngX = VISO(:,4); % delta angle about the X body axis (rad)
    viso_data.dAngY = VISO(:,5); % delta angle about the Y body axis (rad)
    viso_data.dAngZ = VISO(:,6); % delta angle about the Z body axis (rad)
    viso_data.dPosX = VISO(:,7); % delta position along the X body axis (m)
    viso_data.dPosY = VISO(:,8); % delta position along the Y body axis (m)
    viso_data.dPosZ = VISO(:,9); % delta position along the Z body axis (m)
    viso_data.qual = VISO(:,10)/100; % quality from 0 - 1
    save viso_data.mat viso_data;
end

%% save data and clear workspace
clearvars -except baro_data imu_data mag_data gps_data rng_data flow_data viso_data;