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.

matlab/EKF_replay/Common/convert_apm_data.m

@@ -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;