gusucode.com > Simulink——飞行器轨迹恢复源码程序 > Simulink——飞行器轨迹恢复源码程序/Aerial_recovery_demo/mothership_ctrl.m

    function sys = mothership_ctrl(uu,P)

  [c, R, mothership, t] = processInput(uu,P);

  % track orbit
  P.mothership.dir = -1;
  phi_d = trackOrbit(mothership, c, R, P.mothership.dir, P, t);

  % command the roll rate to drive phi to phi_d
  u_phi = sat( P.mothership.k_roll*(phi_d - mothership.phi), P.mothership.phidotbar );
  
  % maintain initial altitude (elevators)
  u_gam = sat( -P.mothership.k_hdot*P.mothership.V*sin(mothership.gam)...
      + P.mothership.k_h*(-P.mothership.d + mothership.d), P.mothership.gamdotbar);  

  sys = [u_phi; u_gam];


%
%=============================================================================
% 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

%
%=============================================================================
% trackOrbit
% track an orbit with center c, radius R, and direction dir (+1=CW, -1=CCW)
% returns the desired roll angle
%=============================================================================
%  
function phi_d = trackOrbit(uav, c, R, dir, P, t);
  persistent chi_diff
  
  V = P.mothership.V;
  
  chi = wrap2pi( uav.chi );
  
  if t < .1,
      chi_diff = 0;
  end
  
  d = norm([uav.n; uav.e] - [c(1);c(2)]);
  gam = wrap2pi( atan2(uav.e-c(2), uav.n-c(1)) );
  
  chi_d = wrap2pi(gam + dir*pi/2 + dir*atan(P.mothership.k_orbit*(d-R)));
  chi_d_dot = V/d*sin(chi-gam) +dir*P.mothership.k_orbit*cos(chi-gam)/(1+(P.mothership.k_orbit*(d-R))^2);

  chi_diff = unwrap(wrap2pi(chi-chi_d), chi_diff);
  phi_d = sat(atan( V/P.g*( chi_d_dot - P.mothership.k_roll*chi_diff ) ), P.mothership.phibar);
  
  
%
%=============================================================================
% wrap2pi
% wrap chi between [0, 2*pi]
%=============================================================================
%  
function chi = wrap2pi(chi)
  while chi > 2*pi,
      chi = chi-2*pi;
  end
  while chi < 0,
      chi = chi+2*pi;
  end  
  
%
%=============================================================================
% unwrap
% ensures that there are not jumps of +- 2pi in the signal
%=============================================================================
%  
function x = unwrap(x, x_old)
  while x-x_old > pi,
      x = x-2*pi;
  end
  while x-x_old < -pi,
      x = x+2*pi;
  end
  
%
%=============================================================================
% process input
%   fill pixel, uav, and target structures
%=============================================================================
function [c,R,mothership,t] = processInput(uu,P)  

  t = uu(end);
  
  c = [uu(1); uu(2)];
  
  R = uu(3);

  % mothership states
  NN = 3;
  mothership.n   = uu(1+NN);       % north position of UAV
  mothership.e   = uu(2+NN);       % east position of UAV
  mothership.d   = uu(3+NN);       % down position of UAV
  mothership.chi = uu(4+NN);       % course
  mothership.phi = uu(5+NN);       % roll
  mothership.gam = uu(6+NN);       % flight path angle (pitch)