1
0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-05 13:24:11 +01:00
LibrePilot/matlab/revo/openpilot2kml.m
2012-06-25 10:43:38 +03:00

280 lines
7.0 KiB
Matlab

function openpilot2kml(matfile)
if ~exist('ge_colorbar', 'file')
msg=['Google Earth Toolbox must be present and included in the Matlab path. For example:'...
10 ' path(path, ''/Users/ponthy/googleearth'''];
error(msg)
end
if nargin==0
[FileName,PathName] = uigetfile('*.mat');
matfile = strcat(PathName,FileName);
end
if ~exist(matfile, 'file')
error('Not a valid matlab file')
end
load(matfile);
%%
t=PositionActual.timestamp/1000;
NED=[PositionActual.North', PositionActual.East', PositionActual.Down'];
BaseECEF = LLA2ECEF([HomeLocation.Latitude*1e-7, HomeLocation.Longitude*1e-7, HomeLocation.Altitude]');
Rne = RneFromLLA([HomeLocation.Latitude*1e-7, HomeLocation.Longitude*1e-7, HomeLocation.Altitude]');
LLA=Base2LLA(NED',BaseECEF,Rne);
gpsPath=LLA(1:2,:)';
gpsAlt=LLA(3,:)';
gpsSpeed=interp1(BaroAirspeed.timestamp/1000, BaroAirspeed.Airspeed, t);
%%
[pathstr, opName] = fileparts(matfile);
kmlFileName=[opName '.kml'];
kmlFolderName=pathstr;
line_seg={}; %#ok<NASGU> %Clear line_seg
gpsTime=t;
maxSpeed=200/3.6;
midSpeed=100/3.6;
%Define colormap
M=jet(round(maxSpeed));
line_seg=cell(round(length(gpsTime)*2),1);
%Create style for waypoints
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];
line_seg{2} = ge_colorbar(gpsPath(1,2), gpsPath(1,1), maxSpeed*(1-exp(log(1/2)/midSpeed*(0:maxSpeed))),...
'cBarBorderWidth',1,...
'cBarFormatStr','%+3.0f',...
'altitudeMode', 'clampToGround',...
'numUnits',10,...
'name','Speed colorbar');
%Add start and stop points
line_seg{3}=ge_point(gpsPath(1,2), gpsPath(1,1), gpsAlt(1), 'altitudeMode', 'clampToGround', 'name', 'Start');
line_seg{4}=ge_point(gpsPath(end,2), gpsPath(end,1), gpsAlt(end), 'altitudeMode', 'clampToGround', 'name', 'Stop');
k=4; m=0; n=0;
%Add text progress output
fprintf('\n\n\n');
str2=sprintf('Completed %3.0f%%', 0);
fprintf([str2 '\n'])
%Create line segments and color based on speed
for i=1:length(gpsTime)-1
m=m+1;
Y=gpsPath(i:i+1,1);
X=gpsPath(i:i+1,2);
Z=[gpsAlt(i) gpsAlt(i)];
logSpeed=maxSpeed*(1-exp(log(1/2)/midSpeed*gpsSpeed(i)));
red=round(M(floor(min(logSpeed+1, length(M))),1)*255);
green=round(M(floor(min(logSpeed+1, length(M))),2)*255);
blue=round(M(floor(min(logSpeed+1, length(M))),3)*255);
tmpHour=floor(t(i)/3600);
tmpMinute=floor((t(i)-tmpHour*3600)/60);
tmpSec=t(i)-tmpHour*3600-tmpMinute*60;
tGPS=[2012 06 23 tmpHour tmpMinute tmpSec];
tStart = datestr( tGPS, 'yyyy-mm-ddTHH:MM:SSZ');
tStop = datestr( tGPS+[0 0 0 0 0 2], 'yyyy-mm-ddTHH:MM:SSZ'); %Visible for two seconds
[line_seg{i+k}]= ge_plot3(X,Y,Z,...
'altitudeMode', 'absolute',...
'timeSpanStart',tStart,...
'timeSpanStop',tStop,...
'lineWidth', 6, ...
'lineColor', [ 'ff' dec2hex(red, 2) dec2hex(green, 2) dec2hex(blue, 2)], ...
'forceAsLine', false, ...
'msgToScreen', false);
if mod(i,10) == 0
%%
k=k+1;
n=n+1;
descriptionCell={'North coords., in [m]', num2str(NED(i,1), '%10.2f');
'East coords., in [m]', num2str(NED(i,2), '%10.2f');
'Height AGL, in [m]', num2str(Z(1), '%10.2f') ;
'Airspeed, in [km/hr]', num2str(gpsSpeed(i)*3.6, '%3.1f')};
[line_seg{i+k}]=ge_point(X(1), Y(1), gpsAlt(i),...
'altitudeMode', 'absolute',...
'extrude', 1, ...
'name', num2str(t(i)-t(1)),...
'timeSpanStart',tStart,...
'timeSpanStop',tStop,...
'styleURL', '#track',...
'iconURL', 'http://earth.google.com/images/kml-icons/track-directional/track-none.png',...
'pointDataCell', descriptionCell);
%%
str1=[];
for j=1:length(str2)+1;
str1=[str1 sprintf('\b')]; %#ok<AGROW>
end
str2=['Completed ' num2str(i/length(t)*100, '%3.0f')];
fprintf([str1 str2 '%%']);
end
end
%Pare down line_seg
%%
line_seg=line_seg(1:i+k);
if 0
%Properly formats XML part of kml file. IS TOO SLOW!
Pref=[]; %#ok<UNRCH>
Pref.StructItem = false;
Pref.XmlEngine = 'String'; %Forces xml_write to use Apache XML engine instead of Matlab XML.
[a,r]=xml_write([], plot_xml, 'b', Pref);
indexFirst=find(r==10, 2);
indexLast=find(r==10, 1, 'last');
r=r(indexFirst(2)+1:indexLast);
[a,s]=xml_write([], point_xml, 'b', Pref);
indexFirst=find(s==10, 2);
indexLast=find(s==10, 1, 'last');
s=s(indexFirst(2)+1:indexLast);
ge_output(kmlFileName, [line_seg{1} r s], 'name', kmlFolderName, 'msgToScreen', true)
else
%%
%Output segments to kml files
ge_output(fullfile(kmlFolderName, kmlFileName), cat(2,line_seg{:}), 'name', opName, 'msgToScreen', true);
end
end
% ===========================
function NED = LLA2Base(LLA,BaseECEF,Rne)
n = size(LLA,2);
ECEF = LLA2ECEF(LLA);
diff = ECEF - repmat(BaseECEF,1,n);
NED = Rne*diff;
end
function LLA = Base2LLA(NED,BaseECEF,Rne)
n = size(NED,2);
diff=Rne'*NED;
ECEF=diff + repmat(BaseECEF,1,n) ;
LLA=zeros(size(NED));
for i=1:n
LLA(:,i)=ECEF2LLA(ECEF(:,i))';
end
end
% ****** convert ECEF to Lat,Lon,Alt (ITERATIVE!) *********
function LLA=ECEF2LLA(ECEF, Alt)
% Altitude parameter is used to prime the iteration.
% A position within 1 meter of the specified LLA
% will be calculated within at most 3 iterations.
% If unknown: Call with any valid altitude
% will compute within at most 5 iterations.
% Suggestion: 0
%
if nargin==1
Alt=0;
end
a = 6378137.0; % Equatorial Radius
e = 8.1819190842622e-2; % Eccentricity
x = ECEF(1);
y = ECEF(2);
z = ECEF(3);
MAX_ITER =10; % should not take more than 5 for valid coordinates
ACCURACY= 1.0e-11; % used to be e-14, but we don't need sub micrometer exact calculations
RAD2DEG=180/pi;
DEG2RAD=1/RAD2DEG;
LLA(2) = RAD2DEG * atan2(y, x);
Lat = DEG2RAD * LLA(1);
esLat = e * sin(Lat);
N = a / sqrt(1 - esLat * esLat);
NplusH = N+Alt;
delta = 1;
iter = 0;
while (((delta > ACCURACY) || (delta < -ACCURACY)) && (iter < MAX_ITER))
delta = Lat - atan(z / (sqrt(x * x + y * y) * (1 - (N * e * e / NplusH))));
Lat = Lat - delta;
esLat = e * sin(Lat);
N = a / sqrt(1 - esLat * esLat);
NplusH = sqrt(x * x + y * y) / cos(Lat);
iter = iter+1;
end
LLA(1) = RAD2DEG * Lat;
LLA(3) = NplusH - N;
end
function ECEF = LLA2ECEF(LLA)
a = 6378137.0; % Equatorial Radius
e = 8.1819190842622e-2; % Eccentricity
n = size(LLA,2);
ECEF = zeros(3,n);
for i=1:n
sinLat = sin(pi*LLA(1,i)/180);
sinLon = sin(pi*LLA(2,i)/180);
cosLat = cos(pi*LLA(1,i)/180);
cosLon = cos(pi*LLA(2,i)/180);
N = a / sqrt(1.0 - e * e * sinLat * sinLat); %prime vertical radius of curvature
ECEF(1,i) = (N + LLA(3,i)) * cosLat * cosLon;
ECEF(2,i) = (N + LLA(3,i)) * cosLat * sinLon;
ECEF(3,i) = ((1 - e * e) * N + LLA(3,i)) * sinLat;
end
end
% ****** find ECEF to NED rotation matrix ********
function Rne=RneFromLLA(LLA)
RAD2DEG=180/pi;
DEG2RAD=1/RAD2DEG;
sinLat = sin(DEG2RAD * LLA(1));
sinLon = sin(DEG2RAD * LLA(2));
cosLat = cos(DEG2RAD * LLA(1));
cosLon = cos(DEG2RAD * LLA(2));
Rne(1,1) = -sinLat * cosLon;
Rne(1,2) = -sinLat * sinLon;
Rne(1,3) = cosLat;
Rne(2,1) = -sinLon;
Rne(2,2) = cosLon;
Rne(2,3) = 0;
Rne(3,1) = -cosLat * cosLon;
Rne(3,2) = -cosLat * sinLon;
Rne(3,3) = -sinLat;
end