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.

cable_drogue_dynamics(t,x,u,flag,P)
function [sys,x0,str,ts] = cable_drogue_dynamics(t,x,u,flag,P)

persistent G;
persistent drogue_CD;

% Simple UAV Dynamics

switch flag,

  %%%%%%%%%%%%%%%%%%
  % Initialization %
  %%%%%%%%%%%%%%%%%%
  case 0,
    [sys,x0,str,ts, G,drogue_CD]=mdlInitializeSizes(P);

  %%%%%%%%%%%%%%%
  % Derivatives %
  %%%%%%%%%%%%%%%
  case 1,
    [sys,drogue_CD]=mdlDerivatives(t,x,u, P, G,drogue_CD);

  %%%%%%%%%%
  % Update %
  %%%%%%%%%%
  case 2,
    sys=mdlUpdate(t,x,u);

  %%%%%%%%%%%
  % Outputs %
  %%%%%%%%%%%
  case 3,
    sys=mdlOutputs(t,x,u,P,drogue_CD);

  %%%%%%%%%%%%%%%%%%%%%%%
  % GetTimeOfNextVarHit %
  %%%%%%%%%%%%%%%%%%%%%%%
  case 4,
    sys=mdlGetTimeOfNextVarHit(t,x,u);

  %%%%%%%%%%%%%
  % Terminate %
  %%%%%%%%%%%%%
  case 9,
    sys=mdlTerminate(t,x,u);

  %%%%%%%%%%%%%%%%%%%%
  % Unexpected flags %
  %%%%%%%%%%%%%%%%%%%%
  otherwise
    error(['Unhandled flag = ',num2str(flag)]);

end

% end sfuntmpl

%
%=============================================================================
% mdlInitializeSizes
% Return the sizes, initial conditions, and sample times for the S-function.
%==========================================================================
%===
%
function [sys,x0,str,ts, G,drogue_CD]=mdlInitializeSizes(P)

%
% call simsizes for a sizes structure, fill it in and convert it to a
% sizes array.
%
% Note that in this example, the values are hard coded.  This is not a
% recommended practice as the characteristics of the block are typically
% defined by the S-function parameters.
%
sizes = simsizes;

sizes.NumContStates  =   3*(P.cable.N -1) ...  % cable position
                       + 3*(P.cable.N -1) ...  % cable velocity
                       + 3 ...    % drogue position, 
                       + 3 ...    % drogue velocity,
                       + 2;       % drogue roll, pitch, 
sizes.NumDiscStates  = 0;
sizes.NumOutputs     = 9 +... % drogue states 
                       + 6*(P.cable.N-1) ... % cable states
                       + 1 ...% cable length
                       +1 ;    % drogue drag coefficient
sizes.NumInputs      = 9 ... % mothership states
                       + 3;  % drogue control inputs (roll rate, pull-up force, velocity)
sizes.DirFeedthrough = 1;
sizes.NumSampleTimes = 1;   % at least one sample time is needed

sys = simsizes(sizes);

%
% initialize the initial conditions
%
  x0 = [];
  if (P.cable.N>=2)
      for i=1:(P.cable.N-1)
          x0 = [x0;...
              P.cable.link.n0(i);...
              P.cable.link.e0(i);...
              P.cable.link.d0(i);...
              P.cable.link.ndot0(i);...
              P.cable.link.edot0(i);...
              P.cable.link.ddot0(i);...
              ];
      end
  end
      
  x0  = [x0;... 
      P.drogue.n;...
      P.drogue.e;...
      P.drogue.d;...
      P.drogue.ndot;...
      P.drogue.edot;...
      P.drogue.ddot;...
      P.drogue.phi;...
      P.drogue.theta;...
      ];
  % Gauss's Principle parameters
    G.A        = zeros(P.cable.N,3*P.cable.N);
    G.M        = diag([P.cable.mass/P.cable.N*ones(3*(P.cable.N-1),1);P.drogue.mass*ones(3,1)]);
    G.a_u      = zeros(3*P.cable.N, 1);
    G.b        = zeros(P.cable.N, 1);
    G.phi      = zeros(P.cable.N, 1);
    G.phi_diff = zeros(3*P.cable.N, P.cable.N);
    G.psi      = zeros(P.cable.N, 1);
    G.psi_diff = zeros(3*P.cable.N, P.cable.N);

    drogue_CD = P.drogue.CD_0;
    %

% str is always an empty matrix
%
str = [];

%
% initialize the array of sample times
%
ts  = [0 0];

% end mdlInitializeSizes

%
%=============================================================================
% mdlDerivatives
% Return the derivatives for the continuous states.
%=============================================================================
%
function [sys,drogue_CD]=mdlDerivatives(t,x,uu,P,G,drogue_CD)
    sys = [];
          
    % mothership states
    NN = 0;
    mothership.n     = uu(1+NN);
    mothership.e     = uu(2+NN);
    mothership.d     = uu(3+NN);
    mothership.chi   = uu(4+NN);
    mothership.phi   = uu(5+NN);
    mothership.gam   = uu(6+NN);
    mothership.nddot = uu(7+NN);
    mothership.eddot = uu(8+NN);
    mothership.dddot = uu(9+NN);
    
    % control inputs to drogue
    NN = 9;
    u_phi  = uu(1+NN);
    u_theta = uu(2+NN);
    u_delta_CD = uu(3+NN);
  
    % drogue states
    NN = 6*(P.cable.N -1);
    drogue.n     = x(1+NN);
    drogue.e     = x(2+NN);
    drogue.d     = x(3+NN); 
    drogue.vn    = x(4+NN);
    drogue.ve    = x(5+NN);
    drogue.vd    = x(6+NN);
    drogue.phi   = x(7+NN);
    drogue.theta = x(8+NN);
    
    if (P.cable.N>=2)  % multiple links are used
        % cable states
        for i = 1:(P.cable.N-1)
            NN = 6*(i-1);
            cable.n(i)  = x(1+NN);
            cable.e(i)  = x(2+NN);
            cable.d(i)  = x(3+NN);
            cable.vn(i) = x(4+NN);
            cable.ve(i) = x(5+NN);
            cable.vd(i) = x(6+NN);
        end

        %%%%%%%%%%%% dynamics of the 1st link which connects to the mothership

        NN = 1;

        ell = [...
            cable.n(NN) - mothership.n;...
            cable.e(NN) - mothership.e;...
            cable.d(NN) - mothership.d;...
            ];
        
        G.phi(NN) = norm(ell)^2 - P.cable.constraint_length^2;
        G.phi_diff(3*(NN-1)+1:3*(NN-1)+3,NN) = ell/norm(ell);

        % line-of-sight rate
        elldot = [...
            cable.vn(NN) - P.mothership.V*cos(mothership.chi)*cos(mothership.gam);...
            cable.ve(NN) - P.mothership.V*sin(mothership.chi)*cos(mothership.gam);...
            cable.vd(NN) + P.mothership.V*sin(mothership.gam);...
            ];
        
        G.psi(NN) = ell'/norm(ell) * elldot;
        G.psi_diff(3*(NN-1)+1:3*(NN-1)+3,NN) = elldot;

        % unconstrained drogue acceleration due to gravity
        g = [0; 0; P.g];

        F_straighten = [0;0;0];
        
        % aerodynamic forces
        F_aeroforce  = aeroforce(P.rho, -ell, P.cable.d, [cable.vn(NN);cable.ve(NN);cable.vd(NN)], P);

        % totally applied forces
        F_total = F_straighten + F_aeroforce;

        % vector "A" of the Gauss's Principle
        G.A(NN,(3*(NN-1)+1):(3*(NN-1)+3)) = ell';

        % unconstrained acceleration
        G.a_u((3*(NN-1)+1):(3*(NN-1)+3),1) = g + F_total/P.cable.mass/P.cable.N;

        % vector "b" of the Gauss's Principle
        G.b (NN,1) = - norm(elldot)^2 +...
            ell'*[mothership.nddot;mothership.eddot;mothership.dddot];

        %%%%%%%%%%%%%%% dynamics of the 2nd-nth links

        if P.cable.N == 2  %%% the nth joint is drogue

            NN = 2;
            ell = [...
                drogue.n- cable.n(NN-1);...
                drogue.e- cable.e(NN-1);...
                drogue.d- cable.d(NN-1)];
            
            G.phi(NN) = norm(ell)^2 - P.cable.constraint_length^2;
            G.phi_diff(3*(NN-1)+1:3*(NN-1)+3,NN) = ell/norm(ell);
            G.phi_diff(3*(NN-2)+1:3*(NN-2)+3,NN) = -ell/norm(ell);

            elldot = [...
                drogue.vn- cable.vn(NN-1);...
                drogue.ve- cable.ve(NN-1);...
                drogue.vd- cable.vd(NN-1)];
            
            G.psi(NN) = ell'/norm(ell) * elldot;
            G.psi_diff(3*(NN-1)+1:3*(NN-1)+3,NN) = elldot;
            G.psi_diff(3*(NN-2)+1:3*(NN-2)+3,NN) = -elldot;

            F_straighten = [0;0;0];

            % drag force on drogue
            V = norm([drogue.vn; drogue.ve; drogue.vd]);
            drogue_CD = drogue_CD + u_delta_CD;
            drogue_CD = sat2side(drogue_CD,P.drogue.CD_bar,P.drogue.CD_bar_lower);
            D_drogue = 0.5*P.rho*V*[drogue.vn; drogue.ve; drogue.vd]*P.drogue.S * (drogue_CD);

            % lift force on drogue
            psi = atan2(ell(2),ell(1));
            u_F = 0;
            L_drogue = Rot_v_to_b(drogue.phi,drogue.theta,psi)'*[0; 0; -u_F];

            % unconstrained acceleration
            G.a_u((3*(NN-1)+1):(3*(NN-1)+3),1) = g + F_straighten/P.cable.mass/P.cable.N + ...
                1/P.drogue.mass*(L_drogue - D_drogue);
            % vector "A" of the Gauss's Principle
            G.A(NN,(3*(NN-2)+1):(3*(NN-2)+3)) = - ell';
            G.A(NN,(3*(NN-1)+1):(3*(NN-1)+3)) =   ell';

            % vector "b" of the Gauss's Principle
            G.b (NN,1) = - norm(elldot)^2;

        else
            %%%% dynamics of the second link
            NN = 2;

            ell = [...
                cable.n(NN)- cable.n(NN-1);...
                cable.e(NN)- cable.e(NN-1);...
                cable.d(NN)- cable.d(NN-1)];
            G.phi(NN) = norm(ell)^2 - P.cable.constraint_length^2;
            G.phi_diff(3*(NN-1)+1:3*(NN-1)+3,NN) = ell/norm(ell);
            G.phi_diff(3*(NN-2)+1:3*(NN-2)+3,NN) = -ell/norm(ell);
            
            elldot = [...
                cable.vn(NN)- cable.vn(NN-1);...
                cable.ve(NN)- cable.ve(NN-1);...
                cable.vd(NN)- cable.vd(NN-1)];
            
            G.psi(NN) = ell'/norm(ell) * elldot;
            G.psi_diff(3*(NN-1)+1:3*(NN-1)+3,NN) = elldot;
            G.psi_diff(3*(NN-2)+1:3*(NN-2)+3,NN) = -elldot;

            if (P.cable.N - NN) <2

                F_straighten = [0;0;0];

            elseif (P.cable.N - NN) ==2
                strait_dir_1 = [...
                    cable.n(NN) - mothership.n;...
                    cable.e(NN) - mothership.e;...
                    cable.d(NN) - mothership.d;...
                    ];
                strait_dir_2 = [...
                    cable.n(NN)- drogue.n;...
                    cable.e(NN)- drogue.e;...
                    cable.d(NN)- drogue.d];

                F_straighten = P.cable.straighten * (strait_dir_1/norm(strait_dir_1)...
                    + strait_dir_2/norm(strait_dir_2));
            else
                strait_dir_1 = [...
                    cable.n(NN) - mothership.n;...
                    cable.e(NN) - mothership.e;...
                    cable.d(NN) - mothership.d;...
                    ];
                strait_dir_2 = [...
                    cable.n(NN)- cable.n(NN+2);...
                    cable.e(NN)- cable.n(NN+2);...
                    cable.d(NN)- cable.n(NN+2)];

                F_straighten = P.cable.straighten * (strait_dir_1/norm(strait_dir_1)...
                    + strait_dir_2/norm(strait_dir_2));
            end
            
            % aerodynamic forces
            F_aeroforce  = aeroforce(P.rho, -ell, P.cable.d, [cable.vn(NN);cable.ve(NN);cable.vd(NN)], P);
            
            % totally applied forces
            F_total = F_straighten + F_aeroforce;           

            % unconstrained acceleration
            G.a_u((3*(NN-1)+1):(3*(NN-1)+3),1) = g + F_total/P.cable.mass/P.cable.N;
            % vector "A" of the Gauss's Principle
            G.A(NN,(3*(NN-2)+1):(3*(NN-2)+3)) = - ell';
            G.A(NN,(3*(NN-1)+1):(3*(NN-1)+3)) =   ell';

            % vector "b" of the Gauss's Principle
            G.b (NN,1) = - norm(elldot)^2;

            %%%%%%%%%%% dynamics of the 3rd~(n-1)th links
            for i = 3:(P.cable.N-1)

                NN = i;

                ell = [...
                    cable.n(NN)- cable.n(NN-1);...
                    cable.e(NN)- cable.e(NN-1);...
                    cable.d(NN)- cable.d(NN-1)];
                G.phi(NN) = norm(ell)^2 - P.cable.constraint_length^2;
                G.phi_diff(3*(NN-1)+1:3*(NN-1)+3,NN) = ell/norm(ell);
                G.phi_diff(3*(NN-2)+1:3*(NN-2)+3,NN) = -ell/norm(ell);
                
                elldot = [...
                    cable.vn(NN)- cable.vn(NN-1);...
                    cable.ve(NN)- cable.ve(NN-1);...
                    cable.vd(NN)- cable.vd(NN-1)];
                
                G.psi(NN) = ell'/norm(ell) * elldot;
                G.psi_diff(3*(NN-1)+1:3*(NN-1)+3,NN) = elldot;
                G.psi_diff(3*(NN-2)+1:3*(NN-2)+3,NN) = -elldot;

                if (P.cable.N - NN) <2
                    F_straighten = [0;0;0];

                elseif (P.cable.N - NN) ==2
                    strait_dir_1 = [...
                        cable.n(NN)- cable.n(NN-2);...
                        cable.e(NN)- cable.n(NN-2);...
                        cable.d(NN)- cable.n(NN-2)];
                    strait_dir_2 = [...
                        cable.n(NN)- drogue.n;...
                        cable.e(NN)- drogue.e;...
                        cable.d(NN)- drogue.d];

                    F_straighten = P.cable.straighten * (strait_dir_1/norm(strait_dir_1)...
                        + strait_dir_2/norm(strait_dir_2));
                else
                    strait_dir_1 = [...
                        cable.n(NN)- cable.n(NN-2);...
                        cable.e(NN)- cable.n(NN-2);...
                        cable.d(NN)- cable.n(NN-2)];
                    strait_dir_2 = [...
                        cable.n(NN)- cable.n(NN+2);...
                        cable.e(NN)- cable.n(NN+2);...
                        cable.d(NN)- cable.n(NN+2)];

                    F_straighten = P.cable.straighten * (strait_dir_1/norm(strait_dir_1)...
                        + strait_dir_2/norm(strait_dir_2));
                end
                
                if NN ~= (P.cable.N-1)
                    % aerodynamic forces
                    F_aeroforce  = aeroforce(P.rho, -ell, P.cable.d, [cable.vn(NN);cable.ve(NN);cable.vd(NN)], P);

                else   % enlarge the drag of the last joint
                    
                    F_aeroforce  = aeroforce(P.rho, -ell, P.cable.d, [cable.vn(NN);cable.ve(NN);cable.vd(NN)], P);
                
                end        %_last_joint
                % totally applied forces
                F_total = F_straighten + F_aeroforce;


                % unconstrained acceleration
                G.a_u((3*(NN-1)+1):(3*(NN-1)+3),1) = g + F_total/P.cable.mass/P.cable.N;

                % vector "A" of the Gauss's Principle
                G.A(NN,(3*(NN-2)+1):(3*(NN-2)+3)) = - ell';
                G.A(NN,(3*(NN-1)+1):(3*(NN-1)+3)) =   ell';

                % vector "b" of the Gauss's Principle
                G.b (NN,1) = - norm(elldot)^2;

            end

            %%%%% dynamics of the nth link i.e. the drogue

            NN = P.cable.N;

            ell = [...
                drogue.n- cable.n(NN-1);...
                drogue.e- cable.e(NN-1);...
                drogue.d- cable.d(NN-1)];
            G.phi(NN) = norm(ell)^2 - P.cable.constraint_length^2;
            G.phi_diff(3*(NN-1)+1:3*(NN-1)+3,NN) = ell/norm(ell);
            G.phi_diff(3*(NN-2)+1:3*(NN-2)+3,NN) = -ell/norm(ell);

            elldot = [...
                drogue.vn- cable.vn(NN-1);...
                drogue.ve- cable.ve(NN-1);...
                drogue.vd- cable.vd(NN-1)];
            
            G.psi(NN) = ell'/norm(ell) * elldot;
            G.psi_diff(3*(NN-1)+1:3*(NN-1)+3,NN) = elldot;
            G.psi_diff(3*(NN-2)+1:3*(NN-2)+3,NN) = -elldot;
            
            F_straighten = [0;0;0];
            
            % drag force on drogue
            V = norm([drogue.vn; drogue.ve; drogue.vd]);
            drogue_CD = drogue_CD+u_delta_CD;
            drogue_CD = sat2side(drogue_CD,P.drogue.CD_bar,P.drogue.CD_bar_lower);
            D_drogue = 0.5*P.rho*V*[drogue.vn; drogue.ve; drogue.vd]*P.drogue.S * (drogue_CD);

            % lift force on drogue
            psi = atan2(ell(2),ell(1));
            u_F = 0.5*P.rho*V^2*P.drogue.S * (P.drogue.CL_0 ); % + u_delta_CL
            L_drogue = Rot_v_to_b(drogue.phi,drogue.theta,psi)'*[0; 0; -u_F];

            % unconstrained acceleration
            G.a_u((3*(NN-1)+1):(3*(NN-1)+3),1) = g + F_straighten/P.cable.mass/P.cable.N + ...
                                                     1/P.drogue.mass*(L_drogue - D_drogue);
            % vector "A" of the Gauss's Principle
            G.A(NN,(3*(NN-2)+1):(3*(NN-2)+3)) = - ell';
            G.A(NN,(3*(NN-1)+1):(3*(NN-1)+3)) =   ell';

            % vector "b" of the Gauss's Principle
            G.b (NN,1) = - norm(elldot)^2;

        end
        
        phidot = lim_sat(drogue.phi, u_phi, P.drogue.phibar, P.drogue.phidotbar);
        % u_phi is the commanded roll rate
        thetadot = lim_sat(drogue.theta, u_theta, P.drogue.thetabar, P.drogue.thetadotbar);   %-u_F/(V+.1)/P.drogue.mass
       
        %%%% compute the matrix that makes sure the constraints not violate
        
        constrained_acceleration = G.a_u + G.M^(-.5) * pinv(G.A*G.M^(-.5)) * (G.b - G.A * G.a_u) - P.cable_spring_1* G.phi_diff * G.phi ...
                                                                           - P.cable_spring_2* G.psi_diff * G.psi;
                                                                       
        for i = 1:(P.cable.N-1)
            
           sys(6*(i-1)+1:6*(i-1)+6,1)  = [cable.vn(i);cable.ve(i);cable.vd(i);constrained_acceleration(3*(i-1)+1:3*(i-1)+3)];
           
        end
        
        i = P.cable.N;
        sys(6*(i-1)+1:6*(i-1)+6,1)  = [drogue.vn;drogue.ve;drogue.vd;constrained_acceleration(3*(i-1)+1:3*(i-1)+3)];
        
        sys = [sys;phidot;thetadot];      
        
    else  %% single link is used
        n     = drogue.n;
        e     = drogue.e;
        d     = drogue.d;
        vn    = drogue.vn;
        ve    = drogue.ve;
        vd    = drogue.vd;
        phi   = drogue.phi;
        theta = drogue.theta;
        
        % compute airspeed
        V = norm([vn; ve; vd]);
        %     angular_rate = V / norm([n,e])

        % line of sight vector
        ell = [...
            mothership.n - n;...
            mothership.e - e;...
            mothership.d - d;...
            ];

        % line-of-sight rate
        elldot = [...
            P.mothership.V*cos(mothership.chi)*cos(mothership.gam) - vn;...
            P.mothership.V*sin(mothership.chi)*cos(mothership.gam) - ve;...
            -P.mothership.V*sin(mothership.gam) - vd;...
            ];

        % unconstrained drogue acceleration due to gravity
        g = [0; 0; P.g];

        % lift force on drogue
        psi = atan2(ell(2),ell(1));
        u_F = 0.5*P.rho*V^2*P.drogue.S * (P.drogue.CL_0 ); % + u_delta_CL
        L_drogue = Rot_v_to_b(phi,theta,psi)'*[0; 0; -u_F];

        % drag force on drogue
        drogue_CD = drogue_CD+u_delta_CD;
        drogue_CD = sat2side(drogue_CD,P.drogue.CD_bar,P.drogue.CD_bar_lower);
        D_drogue = 0.5*P.rho*V*[vn; ve; vd]*P.drogue.S * (drogue_CD);  %  -u_delta_CD

        % unconstrained acceleration of drogue (i.e., without cable)
        a_unconstrained = g + 1/P.drogue.mass*(L_drogue - D_drogue) ;

        % constrained acceleration of the drogue based on Gauss's Principle
        a_constrained = (eye(3)-ell*ell'/P.cable.length^2)*a_unconstrained...
            + ell*ell'/P.cable.length^2*[mothership.nddot; mothership.eddot; mothership.dddot]...
            + ell/P.cable.length^2*norm(elldot)^2;  %norm(ell)^2

        % drogue dynamics
        ndot = vn;
        edot = ve;
        ddot = vd;
      
        nddot = a_constrained(1);
        eddot = a_constrained(2);
        dddot = a_constrained(3);

        phidot = lim_sat(phi, u_phi, P.drogue.phibar, P.drogue.phidotbar);
        % u_phi is the commanded roll rate
        thetadot = lim_sat(theta, u_theta, P.drogue.thetabar, P.drogue.thetadotbar);   %-u_F/(V+.1)/P.drogue.mass
        % The commanded pitch rate is u_F/V/m

        sys = [ndot; edot; ddot; nddot; eddot; dddot; phidot; thetadot];

    end

    

% end mdlDerivatives

%
%=============================================================================
% mdlUpdate
% Handle discrete state updates, sample time hits, and major time step
% requirements.
%=============================================================================
%
function sys=mdlUpdate(t,x,u)

sys = [];

% end mdlUpdate

%
%=============================================================================
% mdlOutputs
% Return the block outputs.
%=============================================================================
%
function sys=mdlOutputs(t,x,uu,P,drogue_CD)

		%  second last joint states
    NN = 6*(P.cable.N -2);
    n  = x(1+NN);
    e  = x(2+NN);
    d  = x(3+NN); 
    
    NN = 6*(P.cable.N -1);
    % drogue states
    drogue.n = x(1+NN);
    drogue.e = x(2+NN);
    drogue.d = x(3+NN);

    % line of sight vector
    ell = [...
        drogue.n - n;...
        drogue.e - e;...
        drogue.d - d;...
        ];
    drogue_psi = atan2(ell(2),ell(1));
sys = [x;drogue_psi;norm(ell);drogue_CD];  % output all UAV states

% end mdlOutputs

%
%=============================================================================
% mdlGetTimeOfNextVarHit
% Return the time of the next hit for this block.  Note that the result is
% absolute time.  Note that this function is only used when you specify a
% variable discrete-time sample time [-2 0] in the sample time array in
% mdlInitializeSizes.
%=============================================================================
%
function sys=mdlGetTimeOfNextVarHit(t,x,u)

sampleTime = 1;    %  Example, set the next hit to be one second later.
sys = t + sampleTime;

% end mdlGetTimeOfNextVarHit

%
%=============================================================================
% mdlTerminate
% Perform any end of simulation tasks.
%=============================================================================
%
function sys=mdlTerminate(t,x,u)

sys = [];

% end mdlTerminate

%
%=============================================================================
% lim_sat
% the integration variable is limited to be within a certain interval and
% the rate of increase is saturated
%=============================================================================
%
function state_dot = lim_sat(state, input, state_limit, rate_limit)

  if ((state>=state_limit)&(input>0))|((state<=-state_limit)&( input<0)) 
     state_dot = 0;
 else
     state_dot = sat(input, rate_limit);
 end


%
%=============================================================================
% sat
% saturate the input u at the constraint c
%=============================================================================
%
function out = sat(u,c)

 if u>c,
     out=c;
 elseif u<-c,
     out=-c;
 else
     out=u;
 end
 

%
function out = sat2side(u,upper_limit,lower_limit)

if u > upper_limit
    out = upper_limit;
elseif u<lower_limit
    out = lower_limit;
else
    out = u;         
end
 
%
%=============================================================================
% aeroforce
% compute the aerodynamic lift and aerodynamic drag of the cable
%=============================================================================
%
function out = aeroforce(rho, L, d, v, P)

if v~=0
    length = norm(L);
    attack_angle = acos(dot(L,v)/(length*norm(v)));
    
    mach = norm(v)/P.sound_speed;
    
    mach_p = mach * cos(attack_angle);
    mach_n = mach * sin(attack_angle);
    
    if mach_p<0.4
        Cf = 0.038 - 0.0425*mach_p;
    else
        Cf = 0.013 + 0.0395*(mach_p-0.085)^2;
    end
    
    Cn = 1.17 + mach_n/40 - mach_n^2/4 + 5*mach_n^3/8;
    
    
    CD = Cf + Cn*(sin(attack_angle))^3;
    CL = Cn * sin(attack_angle)^2 * cos(attack_angle);
    
    temp = cross(cross(v,L),v);
    e_L = - temp/norm(temp);
    
    aerodrag = - .5 * rho * length * d * norm(v)*v * CD;
    aerolift =   .5 * rho * length * d * norm(v)^2 * e_L * CL;
    
    out = aerodrag + aerolift;
else
    out = [0;0;0];
end

%=============================================================================
% aeroforce
% compute the aerodynamic lift and aerodynamic drag of the cable
%=============================================================================
%
function out = aeroforce_last_joint(rho, L, d, v, P)

if v~=0
    length = norm(L);
    attack_angle = acos(dot(L,v)/(length*norm(v)));
    
    CD = P.Cf_last_joint + P.Cn_last_joint*(sin(attack_angle))^3;
    CL = P.Cn_last_joint * sin(attack_angle)^2 * cos(attack_angle);
    
    temp = cross(cross(v,L),v);
    e_L = - temp/norm(temp);
    
    aerodrag = - .5 * rho * length * d * norm(v)*v * CD;
    aerolift =   .5 * rho * length * d * norm(v)^2 * e_L * CL;
    
    out = aerodrag + aerolift;
else
    out = [0;0;0];
end


%%%%%%%%%%%%%%%%%%%%%%
function R = Rot_v_to_b(phi,theta,psi);
% Rotation matrix from vehicle coordinates to body 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_w_to_i(gam,chi)
% Rotation matrix from wind frame to inertial frame
 temp_a = [cos(chi) -sin(chi) 0;...
           sin(chi) cos(chi)  0;...
            0       0         1];
 temp_b = [cos(gam)   0 sin(gam);...
            0         1        0;...
            -sin(gam) 0 cos(gam)];
R = temp_a * temp_b;

Contact us