dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

matlab: Improve conversion of APM log data 

Allow file conversion to work with unequal length GPS and GPA data.
Allow file conversion to work if range, flow and odometry data is not present.
This commit is contained in:
Paul Riseborough 2017-07-24 16:33:12 +10:00
parent df34b43c00
commit e18f92d9c2
1 changed files with 39 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

69
matlab/EKF_replay/Common/convert_apm_data.m
View File

@ -10,6 +10,7 @@ for source_index = 1:length(BARO)
output_index = output_index + 1;
end
end
save baro_data.mat baro_data;
%% extract IMU delta angles and velocity data
clear imu_data;
@ -18,6 +19,7 @@ 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;
@ -32,13 +34,17 @@ for source_index = 1:length(MAG)
output_index = output_index + 1;
end
end
save mag_data.mat mag_data;
%% save GPS daa
clear gps_data;
gps_data.time_us = GPS(:,2);
gps_data.pos_error = GPA(:,4);
gps_data.spd_error = GPA(:,6);
gps_data.hgt_error = GPA(:,5);
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 ));
@ -46,7 +52,7 @@ gps_data.refLLH = [GPS(gps_data.start_index,8);GPS(gps_data.start_index,9);GPS(g
% convert GPS data to NED
deg2rad = pi/180;
for index = 1:length(GPS)
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)];
@ -56,39 +62,42 @@ for index = 1:length(GPS)
end
end
save gps_data.mat gps_data;
%% save range finder data
clear rng_data;
rng_data.time_us = RFND(:,2);
rng_data.dist = RFND(:,3);
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;
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)
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;
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
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;
save baro_data.mat baro_data;
save imu_data.mat imu_data;
save mag_data.mat mag_data;
save gps_data.mat gps_data;
save rng_data.mat rng_data;
save flow_data.mat flow_data;
save viso_data.mat viso_data;