www.gusucode.com > Simulink,飞行器控制源码程序 > Simulink,飞行器控制源码程序/Aerial_recovery_demo/plotenv.m
function [sys,x0,str,ts] = plotenv(t,x,u,flag,P)
%
persistent fig_joint fig_cable;
switch flag,
%%%%%%%%%%%%%%%%%%
% Initialization %
%%%%%%%%%%%%%%%%%%
case 0,
[sys,x0,str,ts,fig_joint,fig_cable] = mdlInitializeSizes(P);
%%%%%%%%%%
% Update %
%%%%%%%%%%
case 2,
[sys,fig_joint,fig_cable] = mdlUpdate(t,x,u,P,fig_joint,fig_cable);
%%%%%%%%%%
% Output %
%%%%%%%%%%
case 3,
sys = mdlOutput(t,x,u,P);
%%%%%%%%%%%%%
% Terminate %
%%%%%%%%%%%%%
case 9,
sys = []; % do nothing
%%%%%%%%%%%%%%%%%%%%
% Unexpected flags %
%%%%%%%%%%%%%%%%%%%%
otherwise
error(['unhandled flag = ',num2str(flag)]);
end
%end dsfunc
%
%=======================================================================
% mdlInitializeSizes
% Return the sizes, initial conditions, and sample times for the S-function.
%=======================================================================
%
function [sys,x0,str,ts,fig_joint,fig_cable] = mdlInitializeSizes(P)
sizes = simsizes;
sizes.NumContStates = 0;
sizes.NumDiscStates = 1+... % initialize
1+... % fig_mothership
1+... % fig_drogue
1; % fig_mav
sizes.NumOutputs = 0;
sizes.NumInputs = 9 ... % mothership
+9 ... % drogue
+ 9 ... % mav
+ 6*(P.cable.N-1); % cable states
sizes.DirFeedthrough = 1;
sizes.NumSampleTimes = 1;
sys = simsizes(sizes);
x0 = zeros(sizes.NumDiscStates,1);
fig_cable = zeros(1,P.cable.N);
fig_joint = zeros(1,(P.cable.N-1));
str = [];
ts = [P.ts 0];
% end mdlInitializeSizes
%
%=======================================================================
% mdlUpdate
% Handle discrete state updates, sample time hits, and major time step
% requirements.
%==============================2=========================================
%
function [xup,fig_joint,fig_cable] = mdlUpdate(t,x,uu,P,fig_joint,fig_cable)
initialize = x(1); % initial graphics
fig_mothership = x(2); % mothership handle
fig_drogue = x(3); % drogue handle
fig_mav = x(4); % mav handle
NN = 0;
mothership.n = uu(1+NN); % north position
mothership.e = uu(2+NN); % east position
mothership.d = uu(3+NN); % down position
mothership.psi = uu(4+NN); % heading angle
mothership.phi = uu(5+NN); % roll angle
mothership.theta = uu(6+NN); % flight path angle
NN = 9; % ignore mothership accelerations
drogue.n = uu(1+NN);
drogue.e = uu(2+NN);
drogue.d = uu(3+NN);
drogue.vn = uu(4+NN);
drogue.ve = uu(5+NN);
drogue.vd = uu(6+NN);
drogue.phi = uu(7+NN);
drogue.theta = uu(8+NN);
drogue.psi = uu(9+NN);
NN = NN+9; %
mav.n = uu(1+NN);
mav.e = uu(2+NN);
mav.d = uu(3+NN);
mav.psi = uu(4+NN);
mav.V = uu(5+NN);
mav.phi = uu(6+NN);
mav.theta = uu(7+NN);
if (P.cable.N>=2)
% cable states
for i = 1:(P.cable.N-1)
NN = 6*(i-1) + 9 + 9 + 9;
cable.n(i) = uu(1+NN);
cable.e(i) = uu(2+NN);
cable.d(i) = uu(3+NN);
cable.vn(i) = uu(4+NN);
cable.ve(i) = uu(5+NN);
cable.vd(i) = uu(6+NN);
end
X(1,1:2) = [mothership.n, cable.n(1)];
Y(1,1:2) = [mothership.e, cable.e(1)];
Z(1,1:2) = [mothership.d, cable.d(1)];
NN = P.cable.N ;
X(NN,1:2) = [cable.n(NN-1), drogue.n];
Y(NN,1:2) = [cable.e(NN-1), drogue.e];
Z(NN,1:2) = [cable.d(NN-1), drogue.d];
if P.cable.N >= 3
for i = 2:(P.cable.N-1)
NN = i ;
X(NN,1:2) = [cable.n(NN-1), cable.n(NN)];
Y(NN,1:2) = [cable.e(NN-1), cable.e(NN)];
Z(NN,1:2) = [cable.d(NN-1), cable.d(NN)];
end
end
end
NN = P.cable.N - 1;
ell = [...
cable.n(NN) - drogue.n;...
cable.e(NN) - drogue.e;...
cable.d(NN) - drogue.d;...
];
drogue.psi = atan2(ell(2),ell(1));
if initialize==0,
% initialize the plot
initialize = 1;
%%%%%%%%%%%%%%%%%%
% create figure
figure(1), clf, hold on
axis([P.x_min, P.x_max, P.y_min, P.y_max, P.z_min, P.z_max]);
% plot mothership, cable, drogue
fig_mothership = drawUav(mothership, P.mothership.size, P, [], 'normal');
fig_drogue = drawUav(drogue, P.drogue.size, P, [], 'normal');
fig_mav = drawUav(mav, P.mav.size, P, [], 'normal');
% Plot cable with multiple links
for i = 1:P.cable.N
fig_cable(i) = drawCable(X(i,1:2),Y(i,1:2),Z(i,1:2), P, [], 'normal');
end
for i = 1:(P.cable.N-1)
fig_joint(i) = drogueJoint([cable.e(i); cable.n(i); -cable.d(i)], P.joint_size, [], '.red', 'normal');
end
grid on
title('Aerial Recovery Demo')
xlabel('East,m');
ylabel('North,m');
zlabel('Altitude,m');
%view(-154,50)
view(24,50)
else % do this at every time step
% plot mothership, cable, drogue
drawUav(mothership, P.mothership.size, P, fig_mothership);
drawUav(drogue, P.drogue.size, P, fig_drogue);
drawUav(mav, P.mav.size, P, fig_mav);
% plot3(mothership.e, mothership.n, -mothership.d);hold on;
% plot3(drogue.e, drogue.n, -drogue.d);
% plot3(mav.e, mav.n, -drogue.d);hold on;
for i = 1:P.cable.N
drawCable(X(i,1:2),Y(i,1:2),Z(i,1:2), P, fig_cable(i));
end
for i = 1:(P.cable.N-1)
drogueJoint([cable.e(i); cable.n(i); -cable.d(i)], P.joint_size, fig_joint(i));
end
drawnow
% drawBreadcrumb(mothership.n,mothership.e,mothership.d,'r');
% drawBreadcrumb(drogue.n,drogue.e,drogue.d,'g');
% plot the trails of the system
plot3(mothership.e, mothership.n, -mothership.d);hold on;
plot3(drogue.e, drogue.n, -drogue.d);hold on;
plot3(mav.e, mav.n, -mav.d);hold on;
end
xup = [initialize; fig_mothership; fig_drogue; fig_mav];
%end mdlUpdate
%=======================================================================
% mdlOutput
%==============================2=========================================
%
function sys = mdlOutput(t,x,u,P)
sys = [];
%----------------------------------------------------------------------
%----------------------------------------------------------------------
% User defined functions
%----------------------------------------------------------------------
%----------------------------------------------------------------------
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function handle =drawCable(X, Y, Z, P, handle, mode);
% cablePlot: plot cable between mothership and drogue
if isempty(handle)
handle = plot3(Y,X,-Z,...
'color','b',...
'EraseMode', mode);
else
set(handle,'XData',Y,'YData',X,'ZData',-Z);
% drawnow
end
function handle = drogueJoint(z, size, handle, color, mode)
if isempty(handle),
handle = plot3(z(1), z(2), z(3), color, 'EraseMode', mode);
else
set(handle,'XData',z(1),'YData',z(2),'ZData',z(3));
% drawnow
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function handle=drawUav(uav, size, P, handle, mode);
% uavPlot: plot UAV attitude at Euler angles phi, theta, psi
[Vert_uav,Face_uav,colors_uav] = uavVertFace(P);
% rescale vertices
Vert_uav = size*Vert_uav;
% rotate vertices by phi, theta, psi
Vert_uav = rotateVert(Vert_uav, pi-uav.phi, -uav.theta, uav.psi);
% transform vertices from NED to XYZ
R = [...
0, 1, 0;...
1, 0, 0;...
0, 0, 1;...
];
Vert_uav = Vert_uav*R;
% translate vertices in XYZ
Vert_uav = translateVert(Vert_uav, [uav.e; uav.n; -uav.d]);
% collect all vertices and faces
V = [...
Vert_uav;...
];
F = [...
Face_uav;...
];
patchcolors = [...
colors_uav;...
];
if isempty(handle),
handle = patch('Vertices', V, 'Faces', F,...
'FaceVertexCData',patchcolors,...
'FaceColor','flat',...
'EraseMode', mode);
else
set(handle,'Vertices',V,'Faces',F);
% drawnow
end
%%%%%%%%%%%%%%%%%%%%%%%
function [V,F,patchcolors] = uavVertFace(P);
% uavData: define patch faces of uav
% vertices of the fuselage
Vert_fuse = [...
P.drawuav.l_fuse/2, P.drawuav.w_fuse/2, P.drawuav.h_fuse/2;...
P.drawuav.l_fuse/2, P.drawuav.w_fuse/2, -P.drawuav.h_fuse/2;...
P.drawuav.l_fuse/2, -P.drawuav.w_fuse/2, -P.drawuav.h_fuse/2;...
P.drawuav.l_fuse/2, -P.drawuav.w_fuse/2, P.drawuav.h_fuse/2;...
-P.drawuav.l_fuse/2, P.drawuav.w_fuse/2, -P.drawuav.h_fuse/2;...
-P.drawuav.l_fuse/2, P.drawuav.w_fuse/2, P.drawuav.h_fuse/2;...
-P.drawuav.l_fuse/2, -P.drawuav.w_fuse/2, -P.drawuav.h_fuse/2;...
-P.drawuav.l_fuse/2, -P.drawuav.w_fuse/2, P.drawuav.h_fuse/2];
% define faces of fuselage
Face_fuse = [...
1, 2, 3, 4;... % front
5, 6, 8, 7;... % back
2, 3, 7, 5;... % top
1, 2, 5, 6;... % right
3, 4, 8, 7;... % left
1, 4, 8, 6]; % bottom
Vert_wing_r = [...
P.drawuav.l_fuse/2-P.drawuav.w_recess, P.drawuav.w_fuse/2, P.drawuav.w_height/2;...
P.drawuav.l_fuse/2-P.drawuav.w_recess, P.drawuav.w_fuse/2, -P.drawuav.w_height/2;...
-P.drawuav.l_fuse/2, P.drawuav.w_fuse/2, 0;...
-P.drawuav.w_back, P.drawuav.w_fuse/2+P.drawuav.w_width, P.drawuav.w_height/2;...
-P.drawuav.w_back, P.drawuav.w_fuse/2+P.drawuav.w_width, -P.drawuav.w_height/2;...
-P.drawuav.w_back-P.drawuav.w_tipwidth, P.drawuav.w_fuse/2+P.drawuav.w_width, 0;...
];
Face_wing_r = 8 + [...
1, 2, 5, 4;... % front
4, 5, 6, 4;... % side
2, 3, 6, 5;... % top
1, 3, 6, 4;... % bottom
];
Vert_wing_l = [...
P.drawuav.l_fuse/2-P.drawuav.w_recess, -P.drawuav.w_fuse/2, P.drawuav.w_height/2;...
P.drawuav.l_fuse/2-P.drawuav.w_recess, -P.drawuav.w_fuse/2, -P.drawuav.w_height/2;...
-P.drawuav.l_fuse/2, -P.drawuav.w_fuse/2, 0;...
-P.drawuav.w_back, -P.drawuav.w_fuse/2-P.drawuav.w_width, P.drawuav.w_height/2;...
-P.drawuav.w_back, -P.drawuav.w_fuse/2-P.drawuav.w_width, -P.drawuav.w_height/2;...
-P.drawuav.w_back-P.drawuav.w_tipwidth, -P.drawuav.w_fuse/2-P.drawuav.w_width, 0;...
];
Face_wing_l = 14 + [...
1, 2, 5, 4;... % front
4, 5, 6, 4;... % side
2, 3, 6, 5;... % top
1, 3, 6, 4;... % bottom
];
Vert_rud_r = [...
-P.drawuav.w_back, P.drawuav.w_fuse/2+P.drawuav.w_width, P.drawuav.w_height/2;...
-P.drawuav.w_back-P.drawuav.w_tipwidth, P.drawuav.w_fuse/2+P.drawuav.w_width, 0;...
-P.drawuav.w_back-P.drawuav.w_tipwidth, P.drawuav.w_fuse/2+P.drawuav.w_width, -P.drawuav.r_height;...
-P.drawuav.w_back-P.drawuav.w_tipwidth+P.drawuav.r_top, P.drawuav.w_fuse/2+P.drawuav.w_width, -P.drawuav.r_height;...
];
Face_rud_r = 20 + [...
1, 2, 3, 4;...
];
Vert_rud_l = [...
-P.drawuav.w_back, -P.drawuav.w_fuse/2-P.drawuav.w_width, P.drawuav.w_height/2;...
-P.drawuav.w_back-P.drawuav.w_tipwidth, -P.drawuav.w_fuse/2-P.drawuav.w_width, 0;...
-P.drawuav.w_back-P.drawuav.w_tipwidth, -P.drawuav.w_fuse/2-P.drawuav.w_width, -P.drawuav.r_height;...
-P.drawuav.w_back-P.drawuav.w_tipwidth+P.drawuav.r_top, -P.drawuav.w_fuse/2-P.drawuav.w_width, -P.drawuav.r_height;...
];
Face_rud_l = 24 + [...
1, 2, 3, 4;...
];
V = [Vert_fuse; Vert_wing_r; Vert_wing_l; Vert_rud_r; Vert_rud_l];
F = [Face_fuse; Face_wing_r; Face_wing_l; Face_rud_r; Face_rud_l];
patchcolors = [...
P.myblue;... % fuselage front
P.myblue;... % back
P.myyellow;... % top
P.myblue;... % right
P.myblue;... % left
P.myblue;... % bottom
P.myblue;... % right wing front
P.mygreen;... % side
P.myblue;... % top
P.myred;... % bottom
P.myblue;... % left wingfront
P.mygreen;... % side
P.myblue;... % top
P.myred;... % bottom
P.mygreen;...% right rudder
P.mygreen;...% left rudder
];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function cityPlot(P)
for i=1:P.num_blocks,
for j=1:P.num_blocks,
C = [...
i*P.street_width + (i-1)*P.building_width + 0.5*P.building_width;...
j*P.street_width + (j-1)*P.building_width + 0.5*P.building_width;...
];
X = [...
C(1) - 0.5*P.building_width;...
C(1) - 0.5*P.building_width;...
C(1) + 0.5*P.building_width;...
C(1) + 0.5*P.building_width;...
];
Y = [...
C(2) - 0.5*P.building_width;...
C(2) + 0.5*P.building_width;...
C(2) + 0.5*P.building_width;...
C(2) - 0.5*P.building_width;...
];
fill(X,Y,'g')
end
end
%%%%%%%%%%%%%%%%%%%%%%%
function Vert=rotateVert(Vert,phi,theta,psi);
% Rotation matrix from body coordinates to vehicle coordinates
% define rotation matrix: body to vehicle
R = Rot_v_to_b(phi, theta, psi)';
% rotate vertices from body frame to vehicle frame
Vert = (R*Vert')';
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% translate vertices by column vector T
function Vert = translateVert(Vert, T)
Vert = Vert + repmat(T', size(Vert,1),1);
% end translateVert
%%%%%%%%%%%%%%%%%%%%%%%
function R = Rot_v_to_b(phi,theta,psi);
% Rotation matrix from body coordinates to vehicle coordinates
Rot_v_to_v1 = [...
cos(psi), sin(psi), 0;...
-sin(psi), cos(psi), 0;...
0, 0, 1;...
];
Rot_v1_to_v2 = [...
cos(theta), 0, -sin(theta);...
0, 1, 0;...
sin(theta), 0, cos(theta);...
];
Rot_v2_to_b = [...
1, 0, 0;...
0, cos(phi), sin(phi);...
0, -sin(phi), cos(phi);...
];
R = Rot_v2_to_b * Rot_v1_to_v2 * Rot_v_to_v1;
%%%%%%%%%%%%%%%%%%%%%%%
function R = Rot_b_to_g(az,el);
% Rotation matrix from body coordinates to gimbal coordinates
Rot_b_to_g1 = [...
cos(az), sin(az), 0;...
-sin(az), cos(az), 0;...
0, 0, 1;...
];
Rot_g1_to_g = [...
cos(el), 0, -sin(el);...
0, 1, 0;...
sin(el), 0, cos(el);...
];
R = Rot_g1_to_g * Rot_b_to_g1;
%%%%%%%%%%%%%%%%%%%%%%%
function w = snap2grid(v1,v2,P)
gam=0;
S = P.num_blocks*(P.building_width+P.street_width);
while ( v1(1)+gam*v2(1) < S )...
& ( v1(1)+gam*v2(1) > 0 )...
& ( v1(2)+gam*v2(2) < S )...
& ( v1(2)+gam*v2(2) > 0 ),
gam = gam + .1;
end
w = v1 + gam*v2;