image thumbnail

Aerial Recovery Concept Demo (Gauss's Principle)

by

 

28 Feb 2013 (Updated )

Demonstrate the concept of aerial recovery of miniature aerial vehicles using a towed-cable system.

plotenv(t,x,u,flag,P)
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;
  
  

Contact us