matlab/revo/openpilot2kml.m

   1 function openpilot2kml(matfile)
   2
   3 if ~exist('ge_colorbar', 'file')
   4         msg=['Google Earth Toolbox must be present and included in the Matlab path. For example:'...
   5                 10 ' path(path, ''/Users/ponthy/googleearth'''];
   6         error(msg)
   7 end
   8
   9 if nargin==0
  10         [FileName,PathName] = uigetfile('*.mat');
  11         matfile = strcat(PathName,FileName);
  12 end
  13
  14 if ~exist(matfile, 'file')
  15         error('Not a valid matlab file')
  16 end
  17 load(matfile);
  18
  19 %%
  20
  21 t=PositionActual.timestamp/1000;
  22
  23 NED=[PositionActual.North', PositionActual.East', PositionActual.Down'];
  24 BaseECEF = LLA2ECEF([HomeLocation.Latitude*1e-7, HomeLocation.Longitude*1e-7, HomeLocation.Altitude]');
  25 Rne = RneFromLLA([HomeLocation.Latitude*1e-7, HomeLocation.Longitude*1e-7, HomeLocation.Altitude]');
  26
  27 LLA=Base2LLA(NED',BaseECEF,Rne);
  28
  29
  30 gpsPath=LLA(1:2,:)';
  31 gpsAlt=LLA(3,:)';
  32 gpsSpeed=interp1(BaroAirspeed.timestamp/1000, BaroAirspeed.Airspeed, t);
  33
  34 %%
  35 [pathstr, opName] = fileparts(matfile);
  36
  37 kmlFileName=[opName '.kml'];
  38
  39
  40 kmlFolderName=pathstr;
  41
  42 line_seg={}; %#ok<NASGU> %Clear line_seg
  43 gpsTime=t;
  44
  45 maxSpeed=200/3.6;
  46 midSpeed=100/3.6;
  47
  48 %Define colormap
  49 M=jet(round(maxSpeed));
  50
  51 line_seg=cell(round(length(gpsTime)*2),1);
  52
  53 %Create style for waypoints
  54 line_seg{1}= ['<Style id="track"> <IconStyle> <scale>1.2</scale> <Icon> <href>http://earth.google.com/images/kml-icons/track-directional/track-none.png</href> </Icon> </IconStyle> </Style>' 10];
  55
  56 line_seg{2} = ge_colorbar(gpsPath(1,2), gpsPath(1,1), maxSpeed*(1-exp(log(1/2)/midSpeed*(0:maxSpeed))),...
  57         'cBarBorderWidth',1,...
  58         'cBarFormatStr','%+3.0f',...
  59         'altitudeMode', 'clampToGround',...
  60         'numUnits',10,...
  61         'name','Speed colorbar');
  62
  63 %Add start and stop points
  64 line_seg{3}=ge_point(gpsPath(1,2), gpsPath(1,1), gpsAlt(1), 'altitudeMode', 'clampToGround', 'name', 'Start');
  65 line_seg{4}=ge_point(gpsPath(end,2), gpsPath(end,1), gpsAlt(end), 'altitudeMode', 'clampToGround', 'name', 'Stop');
  66 k=4; m=0; n=0;
  67
  68 %Add text progress output
  69 fprintf('\n\n\n');
  70 str2=sprintf('Completed %3.0f%%', 0);
  71 fprintf([str2 '\n'])
  72
  73 %Create line segments and color based on speed
  74 for i=1:length(gpsTime)-1
  75         m=m+1;
  76         Y=gpsPath(i:i+1,1);
  77         X=gpsPath(i:i+1,2);
  78         Z=[gpsAlt(i) gpsAlt(i)];
  79         logSpeed=maxSpeed*(1-exp(log(1/2)/midSpeed*gpsSpeed(i)));
  80
  81         red=round(M(floor(min(logSpeed+1, length(M))),1)*255);
  82         green=round(M(floor(min(logSpeed+1, length(M))),2)*255);
  83         blue=round(M(floor(min(logSpeed+1, length(M))),3)*255);
  84
  85         tmpHour=floor(t(i)/3600);
  86         tmpMinute=floor((t(i)-tmpHour*3600)/60);
  87         tmpSec=t(i)-tmpHour*3600-tmpMinute*60;
  88         tGPS=[2012 06 23 tmpHour tmpMinute tmpSec];
  89         tStart = datestr( tGPS, 'yyyy-mm-ddTHH:MM:SSZ');
  90         tStop  = datestr( tGPS+[0 0 0 0 0 2], 'yyyy-mm-ddTHH:MM:SSZ'); %Visible for two seconds
  91
  92         [line_seg{i+k}]= ge_plot3(X,Y,Z,...
  93                 'altitudeMode', 'absolute',...
  94                 'timeSpanStart',tStart,...
  95                 'timeSpanStop',tStop,...
  96                 'lineWidth', 6, ...
  97                 'lineColor', [ 'ff' dec2hex(red, 2) dec2hex(green, 2) dec2hex(blue, 2)], ...
  98                 'forceAsLine', false, ...
  99                 'msgToScreen', false);
 100
 101         if mod(i,10) == 0
 102                 %%
 103                 k=k+1;
 104                 n=n+1;
 105
 106                 descriptionCell={'North coords., in [m]', num2str(NED(i,1), '%10.2f');
 107                         'East coords., in [m]', num2str(NED(i,2), '%10.2f');
 108                         'Height AGL, in [m]', num2str(Z(1), '%10.2f') ;
 109                         'Airspeed, in [km/hr]', num2str(gpsSpeed(i)*3.6, '%3.1f')};
 110
 111                 [line_seg{i+k}]=ge_point(X(1), Y(1), gpsAlt(i),...
 112                         'altitudeMode', 'absolute',...
 113                         'extrude', 1, ...
 114                         'name', num2str(t(i)-t(1)),...
 115                         'timeSpanStart',tStart,...
 116                         'timeSpanStop',tStop,...
 117                         'styleURL', '#track',...
 118                         'iconURL', 'http://earth.google.com/images/kml-icons/track-directional/track-none.png',...
 119                         'pointDataCell', descriptionCell);
 120
 121                 %%
 122                 str1=[];
 123                 for j=1:length(str2)+1;
 124                         str1=[str1 sprintf('\b')]; %#ok<AGROW>
 125                 end
 126                 str2=['Completed ' num2str(i/length(t)*100, '%3.0f')];
 127
 128                 fprintf([str1 str2  '%%']);
 129
 130         end
 131 end
 132 %Pare down line_seg
 133 %%
 134 line_seg=line_seg(1:i+k);
 135
 136 if 0
 137         %Properly formats XML part of kml file. IS TOO SLOW!
 138         Pref=[]; %#ok<UNRCH>
 139         Pref.StructItem = false;
 140         Pref.XmlEngine = 'String';  %Forces xml_write to use Apache XML engine instead of Matlab XML.
 141
 142         [a,r]=xml_write([], plot_xml, 'b', Pref);
 143         indexFirst=find(r==10, 2);
 144         indexLast=find(r==10, 1, 'last');
 145
 146         r=r(indexFirst(2)+1:indexLast);
 147
 148         [a,s]=xml_write([], point_xml, 'b', Pref);
 149         indexFirst=find(s==10, 2);
 150         indexLast=find(s==10, 1, 'last');
 151
 152         s=s(indexFirst(2)+1:indexLast);
 153
 154         ge_output(kmlFileName, [line_seg{1} r s], 'name', kmlFolderName, 'msgToScreen', true)
 155 else
 156         %%
 157         %Output segments to kml files
 158         ge_output(fullfile(kmlFolderName, kmlFileName), cat(2,line_seg{:}), 'name', opName, 'msgToScreen', true);
 159 end
 160
 161 end
 162
 163 % ===========================
 164
 165
 166 function NED = LLA2Base(LLA,BaseECEF,Rne)
 167 n = size(LLA,2);
 168
 169 ECEF = LLA2ECEF(LLA);
 170
 171 diff = ECEF - repmat(BaseECEF,1,n);
 172
 173 NED = Rne*diff;
 174 end
 175
 176 function LLA = Base2LLA(NED,BaseECEF,Rne)
 177 n = size(NED,2);
 178
 179 diff=Rne'*NED;
 180 ECEF=diff + repmat(BaseECEF,1,n) ;
 181
 182 LLA=zeros(size(NED));
 183 for i=1:n
 184         LLA(:,i)=ECEF2LLA(ECEF(:,i))';
 185 end
 186 end
 187
 188
 189 % ****** convert ECEF to Lat,Lon,Alt (ITERATIVE!) *********
 190 function LLA=ECEF2LLA(ECEF, Alt)
 191 % Altitude parameter is used to prime the iteration.
 192 %   A position within 1 meter of the specified LLA
 193 %        will be calculated within at most 3 iterations.
 194 %        If unknown: Call with any valid altitude
 195 %        will compute within at most 5 iterations.
 196 %        Suggestion: 0
 197 %
 198
 199 if nargin==1
 200         Alt=0;
 201 end
 202
 203 a = 6378137.0;            % Equatorial Radius
 204 e = 8.1819190842622e-2;  % Eccentricity
 205 x = ECEF(1);
 206 y = ECEF(2);
 207 z = ECEF(3);
 208
 209 MAX_ITER =10;            % should not take more than 5 for valid coordinates
 210 ACCURACY= 1.0e-11; % used to be e-14, but we don't need sub micrometer exact calculations
 211 RAD2DEG=180/pi;
 212 DEG2RAD=1/RAD2DEG;
 213
 214 LLA(2) = RAD2DEG * atan2(y, x);
 215 Lat = DEG2RAD * LLA(1);
 216 esLat = e * sin(Lat);
 217 N = a / sqrt(1 - esLat * esLat);
 218 NplusH = N+Alt;
 219 delta = 1;
 220 iter = 0;
 221
 222 while (((delta > ACCURACY) || (delta < -ACCURACY)) && (iter < MAX_ITER))
 223         delta = Lat - atan(z / (sqrt(x * x + y * y) * (1 - (N * e * e / NplusH))));
 224         Lat = Lat - delta;
 225         esLat = e * sin(Lat);
 226         N = a / sqrt(1 - esLat * esLat);
 227         NplusH = sqrt(x * x + y * y) / cos(Lat);
 228         iter = iter+1;
 229 end
 230
 231 LLA(1) = RAD2DEG * Lat;
 232 LLA(3) = NplusH - N;
 233
 234
 235 end
 236
 237 function ECEF = LLA2ECEF(LLA)
 238
 239 a = 6378137.0;  % Equatorial Radius
 240 e = 8.1819190842622e-2; % Eccentricity
 241
 242 n = size(LLA,2);
 243 ECEF = zeros(3,n);
 244
 245 for i=1:n
 246         sinLat = sin(pi*LLA(1,i)/180);
 247         sinLon = sin(pi*LLA(2,i)/180);
 248         cosLat = cos(pi*LLA(1,i)/180);
 249         cosLon = cos(pi*LLA(2,i)/180);
 250
 251         N = a / sqrt(1.0 - e * e * sinLat * sinLat); %prime vertical radius of curvature
 252
 253         ECEF(1,i) = (N + LLA(3,i)) * cosLat * cosLon;
 254         ECEF(2,i) = (N + LLA(3,i)) * cosLat * sinLon;
 255         ECEF(3,i) = ((1 - e * e) * N + LLA(3,i)) * sinLat;
 256 end
 257 end
 258
 259 % ****** find ECEF to NED rotation matrix ********
 260 function  Rne=RneFromLLA(LLA)
 261 RAD2DEG=180/pi;
 262 DEG2RAD=1/RAD2DEG;
 263
 264 sinLat = sin(DEG2RAD * LLA(1));
 265 sinLon = sin(DEG2RAD * LLA(2));
 266 cosLat = cos(DEG2RAD * LLA(1));
 267 cosLon = cos(DEG2RAD * LLA(2));
 268
 269 Rne(1,1) = -sinLat * cosLon;
 270 Rne(1,2) = -sinLat * sinLon;
 271 Rne(1,3) = cosLat;
 272 Rne(2,1) = -sinLon;
 273 Rne(2,2) = cosLon;
 274 Rne(2,3) = 0;
 275 Rne(3,1) = -cosLat * cosLon;
 276 Rne(3,2) = -cosLat * sinLon;
 277 Rne(3,3) = -sinLat;
 278 end
 279